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I.  INTRODUCTION 

A.  RESEARCH  MOTIVATION 

The  uses  of  robotics  in  the  modern  world  are  nearly  limitless.  From 
manufacturing  applications  to  high-precision  surgery,  from  unmanned  aerial  vehicles 
(UAVs)  to  planetary  rovers  to  satellites  with  manipulator  arms,  applications  for  robotics 
abound.  Within  any  one  of  these  application  areas,  specialized  areas  of  investigation 
emerge.  A  roboticist  must  strive  to  understand  hardware  like  motors  and  sensors, 
software  for  control  and  simulation,  the  under-lying  multi-body  mechanics,  and  the 
various  techniques  for  implementing  these  pieces  into  a  functional  system 
(mechatronics).  In  areas  that  are  inherently  dangerous  or  inaccessible  to  humans,  like 
outer  space,  robotic  applications  are  a  natural  fit. 

Since  the  1980s,  the  notion  of  using  robotic  servicing  satellites  to  extend  the 
lifetime  of  on-orbit  assets  has  generated  an  increasingly  large  field  of  interest.  Traditional 
satellites  continue  to  be  very  large  and  very  expensive,  typically  the  result  of  many  years 
of  substantial  investment.  During  a  satellite’s  operational  lifetime,  any  number  of 
electrical,  mechanical,  thermal,  propulsion  or  structural  problems  may  arise  and  render  it 
incapable  of  performing  its  mission.  A  servicing  satellite  equipped  with  a  robotic 
manipulator  may  be  able  to  correct  or  mitigate  these  problems,  and  this  technology  has 
been  demonstrated,  most  notably  in  Japan’s  Experimental  Test  Satellite  7  (ETS-7),  and 
the  United  States’  Orbital  Express  mission.  Servicing  satellites  such  as  these  could  assist 
in  the  deployment  of  a  stuck  antenna  or  solar  panel,  the  replenishment  of  an  empty 
propellant  tank,  or  the  stabilization  of  a  tumbling  spacecraft  [1],  [2].  For  failed  spacecraft 
and  for  other  pieces  of  space  junk,  a  satellite  with  a  manipulator  arm  could  gain  control 
of  the  debris  and  assist  its  deorbit  or  tug  it  to  a  location  where  it  will  not  interfere  with 
active  missions. 

Among  terrestrial  manipulators,  fixed-based  manipulators  for  use  in  industrial 
applications  are  the  most  common  type.  Over  the  years,  the  mechanics  and  control  theory 
necessary  to  operate  these  industrial  systems  were  adapted  for  use  with  spacecraft 
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manipulators,  taking  into  account  the  very  important  consideration  that  a  satellite  with  a 
manipulator  cannot  be  treated  as  a  fixed-base  manipulator  in  all  cases.  If,  for  example, 
the  manipulator  is  sufficiently  large  with  respect  to  the  base  spacecraft,  or  if  the 
manipulator  moves  faster  than  a  given  speed,  the  action  of  the  arm  will  impart  significant 
reaction  onto  the  base  and  vice  versa. 

To  account  for  this  highly  nonlinear  dynamic  coupling,  then,  the  kinematics  and 
the  dynamics  of  the  system  must  be  modeled  and  simulated  numerically  to  understand 
how  the  system  will  behave  for  a  given  number  of  manipulator  links.  Further,  since  one 
cannot  simulate  all  physical  effects  of  the  environment  (friction  forces  or  sensor  noise, 
for  example),  a  hardware  validation  of  the  model  is  necessary.  Because  of  the  high  mass 
penalty  of  putting  spacecraft  into  orbit,  it  is  desirable  to  launch  the  smallest  satellite 
capable  of  perfonning  a  given  mission — even  if  using  a  smaller  satellite  means  greater 
complexity  in  the  dynamical  behavior  of  the  system.  With  this  goal  in  mind,  research  at 
the  Naval  Postgraduate  School  (NPS)  Spacecraft  Robotics  Laboratory  (SRL)  on  free- 
floating  spacecraft  simulators  with  multiple-link  robotic  arms  has  been  ongoing. 

B.  STATE  OF  THE  ART 

1.  An  Overarching  Context 

Space-based  manipulators  and  the  experimental  models  used  for  development  and 
simulation  have  a  long  heritage,  but  before  discussing  the  very  specific  application  of  a 
satellite-based  robotic  manipulator,  a  discussion  of  the  definition  of  robotics  and 
consideration  of  the  state-of-the  art  of  robotics  is  useful.  The  experimental  and 
operational  heritage  of  the  current  research  effort  is  discussed,  including  the  early 
examples  of  floating  manipulators,  manipulator  campaigns  aboard  the  Space 
Transportation  System  (STS)  and  the  International  Space  Station  (ISS),  the  Experimental 
Test  Satellite  7  (ETS-VII)  and  Orbital  Express  missions,  and  ongoing  research  efforts 
within  academia. 

In  her  book,  The  Robotics  Primer ,  Dr.  Maja  Mataric,  the  Vice  Dean  for  Research 
at  the  University  of  Southern  California’s  (USC)  Viterbi  School  of  Engineering,  rigidly 
defines  a  robot  as  “an  autonomous  system  which  exists  in  the  physical  world,  can  sense 
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its  environment,  and  can  act  on  it  to  achieve  some  goals”  [3].  While  this  definition  allows 
for  systems  of  varying  levels  of  autonomy,  Mataric  goes  on  to  deliberately  exclude 
manipulators  that  are  controlled  via  tele-operation — that  is,  an  operator  controls  the 
manipulator  remotely.  This  exclusion  contradicts  a  reality  that  is  evident  in  published 
literature:  the  larger  robotics  community  considers  tele-operated  manipulators  as  robots. 
As  evidence,  the  2015  Institute  of  Electrical  and  Electronics  Engineers’  (IEEE) 
International  Conference  on  Robotics  and  Automation  (ICRA)  and  International 
Workshop  on  Intelligent  Robots  and  Systems  (IROS),  two  very  prominent  conferences, 
included  multiple  papers  on  tele-operation,  particularly  as  it  relates  to  high-precision 
surgery  (for  example,  [4],  [5])  but  also  relating  to  space  operations  [6].  Indeed,  as  a 
general  observation,  the  literature  of  space  manipulators  (tele-operated  or  not)  proclaims 
their  inclusion  in  the  field  of  robotics.  Therefore,  this  thesis  keeps  with  the  larger 
community  in  its  consideration  of  tele-operated  manipulators  as  a  valid  subset  of 
robotics. 

Even  highly  independent,  non-tele-operated  robots  depend  upon  a  human  user  for 
varying  degrees  of  input  (that  is,  they  have  varying  levels  of  autonomy).  At  the  more 
sophisticated  level,  Rethink  Robotics  touts  its  industrial  robot  Baxter  as  “trained,  not 
programmed”  [7].  Baxter  is  a  robot  that  specializes  in  pick-and-place  tasks,  but  unlike 
other  robots,  a  user  can  reconfigure  Baxter  to  perform  a  new  task  simply  by  manually 
repositioning  its  manipulators  in  the  desired  sequence  [8],  If  an  object  is  out  of  its 
expected  place — as  it  moves  along  a  conveyor  belt,  for  example — a  camera  mounted  in 
Baxter’s  wrist  will  seek  the  misplaced  object  and  automatically  adjust  its  behavior,  and  if 
a  human  co-worker  gets  in  the  way,  Baxter  will  sense  the  collision  and  safely  cease 
operation  [8]. 

Two  versions  of  the  Baxter  robot  with  different  end  effectors  are  shown  in  their 
industrial  setting  in  Figure  1.  Industrial  robots  offer  two  sharp  contrasts  to  the  robotics 
found  on  spacecraft.  First,  Baxter  has  a  fixed  base,  so  motion  of  its  manipulators  does  not 
impart  a  reaction  motion  by  the  base.  Second,  the  complexity  of  Baxter’s  behavior  is  not 
an  option  when  dealing  with  a  manipulator  on  an  unmanned  satellite  because  there  is  no 
human  nearby  to  “teach”  it.  Since  autonomous  operation  of  a  satellite -based  robotic 
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manipulator  is  a  highly  sophisticated  endeavor,  most  spacecraft  robotic  manipulators 
have  historically  been  operated  via  tele-operation. 


Figure  1.  Two  Baxter  Robots  with  Different  End  Effectors.  Source:  [7]. 

Earth-bound  tele-operated  robots,  like  Baxter  and  other  more  autonomous  earth- 
bound  manipulators,  are  (not  surprisingly)  capable  of  higher  precision  and  higher 
complexity  functions  than  their  space-bound  counterparts.  In  particular,  the  field  of 
surgery  via  tele-robotics  has  flourished,  but  efforts  are  underway  to  make  the  process 
more  autonomous.  The  manipulators  pictured  in  Figure  2  employ  as  end  effectors  a  6  mm 
grasper  and  a  10  mm  pair  of  scissors  to  remove  simulated  tumors  from  simulated  tissue 
(left)  and  to  remove  simulated  damaged  skin  (right)  [9],  These  manipulators  are 
programmed  by  a  technique  that  “involves  observing  human-operated  demonstrations 
into  motion  sequences  and  transition  conditions”  and  are  capable  of  detecting  the 
simulated  tumors  and  dead  skin  and  acting  accordingly  [9],  Like  the  manual  repositioning 
of  Baxter,  learning  by  observation  presents  intriguing  possibilities  for  the  field  of 
spacecraft  robotics,  but  because  of  the  dynamic  behavior  of  a  spacecraft-manipulator 
system,  such  a  system  will  likely  never  be  able  to  achieve  the  precision  of  surgical 
robotics. 
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Figure  2.  Precise  Surgical  Manipulators.  Source:  [9]. 


2.  Spacecraft  Based  Manipulators  in  Experiment  and  Application 

With  examples  of  highly-programmable  and  highly  precise  robotic  manipulators 
in  mind,  it  is  now  useful  to  consider  a  brief  history  of  spacecraft-based  manipulators,  both 
those  used  in  the  simulation  environment  and  those  actually  employed  in  space.  The  most 
recognized  space  manipulator  is  the  Shuttle  Remote  Manipulator  System  (SRMS)  or  the 
Canadarm,  the  original  version  of  which  flew  on  the  second  Shuttle  mission  (STS-2, 
Columbia)  in  1981  (Figure  3)  [10].  The  four  subsequent  Canadarms,  including  the  current 
version  aboard  the  International  Space  Station  (colloquially  referred  to  as  Canadarm2), 
have  employed  programmed  routines  and  tele-operation  by  astronauts  [11].  According  to 
interviews  with  former  astronauts  Dr.  Jim  Newman  and  Captain  (Retired)  Dan  Bursch, 
the  light  weight  of  the  manipulators  relative  to  their  host  spacecraft,  their  relatively  slow 
motion,  and  carefully  planned  maneuvers  mitigate  the  problem  of  dynamically  coupling 
the  manipulator’s  motion  to  that  of  the  spacecraft. 

Because  of  its  enormous  size,  the  ISS  is  less  susceptible  to  dynamic  coupling 
from  its  manipulators  than  was  the  Shuttle.  Specifically,  “Canadarm2  has  a  mass  of  1,800 
kg,  in  comparison  to  the  ISS,  which  has  a  mass  of  approximately  420,000  kg,”  and  its  tip 
speed  “during  station  assembly  is  2  cm/s,  while  during  [Extra  Vehicular  Activity]  support 
the  maximum  speed  is  15  cm/s”  [12].  In  essence,  the  Canadarm2  is  so  small  relative  to 
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the  ISS  and  moves  so  slowly  within  its  preplanned  maneuver  envelope  that  the  system 
can  be  treated  as  a  fixed-based  manipulator  from  the  perspective  of  system  dynamics. 
However,  with  the  trend  toward  smaller,  more  capable  satellites  in  mind,  questions  about 
how  a  manipulator’s  motion  would  affect  the  dynamics  of  a  “small”  spacecraft  became 
an  area  ripe  for  investigation. 


Figure  3.  Deployment  of  the  original  Canadarm  from  Columbia.  Source:  [11]. 

One  of  the  earliest  experimental  campaigns  on  small  spacecraft-manipulator 
systems  began  in  1987  under  Dr.  Kazuya  Yoshida  at  Japan’s  Tohuku  University  with  the 
Experimental  Free-Floating  Robot  Satellite  (EFFORTS-1,  Figure  4)  [13].  By  this  time, 
experimentation  on  spacecraft  robotics  had  taken  place  in  multiple  different  experimental 
environments  including  parabolic  airplane  flight,  neutral  buoyancy  pools,  and  tethered 
suspension  [13].  The  best  testing  environment,  of  course,  would  have  been  outer  space, 
but  limited  access  prevented  such  an  opportunity  to  all  but  a  few.  The  most  practical 
method — the  one  employed  by  Yoshida,  by  the  experimental  campaign  for  this  thesis, 
and  still  the  most  common  method  for  spacecraft  robotics  experimentation  [14] — was 
employing  a  flat,  smooth  surface  over  which  a  robot  could  float  on  air  bearings. 
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Figure  4.  EFFORTS-1  Floating  Via  Air  Bearings.  Source:  [13]. 

In  1992,  the  Japanese  research  team  of  Kazuo  Machida,  Yoshitsugu  Toda,  and 
Toshiaki  Itawa  published  results  of  their  experimentation  on  a  multiple-armed  spacecraft 
simulator,  the  Astronaut  Reference  Flying  Robot  (ARFR,  Figure  5)  [15].  It  was  a  tele- 
operated  system  aimed  at  “performing  in-orbit  servicing  in  the  manner  of  an  astronaut 
with  a  manned  maneuvering  unit”  [15].  Like  EFFORTS-1,  ARFR  was  developed  as  a 
free-floating  robot.  Meanwhile,  Yoshida  was  working  on  EFFORTS-2,  a  two-armed 
floating  spacecraft-manipulator  system  that  was  functionally  similar  to  the  ARFR, 
including  its  dependence  on  tele-operation  [13].  Experimental  campaigns  resembling 
ARFR  and  EFFORTS  would  continue  throughout  the  1990s — many  at  the  university 
level. 

One  experimental  campaign  that  was  fortunate  enough  to  be  tested  in  space  was 
Gennany’s  Robot  Technology  Experiment  (ROTEX).  In  1993,  ROTEX  flew  aboard  the 
Shuttle  and  demonstrated  the  capability  for  automatic  behavior,  tele-operation  from  the 
Shuttle,  tele-operation  from  Earth  and  “tele-sensor  programming  (i.e.,  learning  by 
showing  in  a  completely  simulated  world  on-ground,  including  the  sensory  perception 
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with  sensor-based  execution  later  on-board)”  [16].  As  a  small,  on-board  manipulator,  the 
dynamics  of  ROTEX,  like  the  dynamics  of  the  unloaded  Canadarm,  were  inconsequential 
to  the  dynamics  of  the  spacecraft,  but  the  ROTEX  experiment  demonstrated  that 
automation  of  robotics,  the  tele-operation  of  robotics,  and  the  desire  to  combine  these 
technologies  on  a  small  spacecraft  were  developing  in  parallel.  In  fact,  these  technologies 
were  actually  converging. 
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Figure  5.  Astronaut  Reference  Flying  Robot  (ARFR).  Source:  [15]. 


The  initial  convergence  came  in  the  form  of  Japan’s  Engineering  Test  Satellite  7 
(ETS-VII),  which  was  launched  November  28,  1997  (Figure  6)  [17].  ETS-VII  actually 
consisted  of  two  satellites,  a  2.5-ton  chaser  and  a  0.4-ton  target  spacecraft  [1]  and 
accomplished  the  “autonomous  [rendezvous  and  docking]  by  [an]  unmanned  space 
vehicle  for  [the]  first  time  in  the  world”  [18].  With  a  relative  mass  between  the  spacecraft 
and  the  manipulator  of  24,  the  manipulator  was  small  enough  that  its  motion  did  not 
significantly  affect  the  motion  of  the  base  [12].  Further,  the  robotic  manipulator  missions 
were  conducted  by  tele-operation  from  the  ground  [17].  Thus,  while  ETS-VII  was  a  very 
important  step  in  the  evolution,  it  falls  short  of  a  small  spacecraft  with  relatively  large 
manipulator  capable  of  autonomous  behavior. 
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Figure  6.  ETS-VII.  Source:  [17]. 


In  March  of  2007,  the  Defense  Advanced  Research  Project  Agency  (DARPA)  of 
the  United  States  launched  the  Orbital  Express  mission  [2],  In  many  ways,  the 
experimental  campaign  resembled  that  of  ETS-VII.  Orbital  Express  consisted  of  a  chaser 
vehicle  named  ASTRO  with  a  six-degree  of  freedom  manipulator  named  the  Orbital 
Express  Demonstration  Manipulator  System  (OEDMS)  and  a  target  vehicle  named 
NextSat  [2].  Like  ETS-VII,  the  relatively  large  size  of  ASTRO  compared  to  OEDMS — a 
relative  mass  ratio  of  15 — prevented  significant  effects  of  the  manipulator’s  motion  from 
affecting  the  base  spacecraft  itself  [12],  Unlike  ETS-VII,  however,  Orbital  Express  was 
capable  of  conducting  operations  with  “various  levels  of  autonomy,”  including 
“operations  where  only  a  single  command  was  sent  to  initiate  the  test  scenario”  [2]. 
Space-based  robotic  manipulators  without  tele-operation  had  arrived. 

A  spacecraft  with  a  small  relative  mass  compared  to  its  manipulator  has  yet  to  fly 
in  space,  but  efforts  are  underway  at  the  University  of  Maryland’s  Space  Systems 
Laboratory  to  develop  the  Dynamic  Manipulation  Flight  Experiment  Microsat 
(DYMAFLEX,  Figure  7).  Unlike  ETS-VII  or  Orbital  Express,  DYMAFLEX  is  a  small 
satellite  with  a  robotic  manipulator  that  is  ~14%  of  the  mass  of  the  combined  system  [19] 
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(a  relative  mass  ratio  of  5.3)  [11].  Such  a  small  satellite  would  provide  a  cheaper 
alternative  to  launching  behemoth  satellites  with  relatively  small  manipulators,  and  if  the 
coupled  dynamics  can  be  controlled  and  the  autonomous  behavior  accomplished, 
DYMAFLEX  could  represent  the  convergence  of  small  satellite  and  autonomous  robotics 
sought  since  Yoshida  began  his  experiments  in  the  1980s. 


Figure  7.  DYMAFLEX  with  Deployed  Antennas.  Source:  [19]. 

3.  Efforts  at  the  Naval  Postgraduate  School  (NPS)  Spacecraft  Robotics 
Laboratory  (SRL) 

The  current  efforts  at  the  NPS  SRL  follows  the  same  basic  approach  as  many 
other  robotics  efforts  throughout  academia.  The  basic  methodology  consists  of  four 
phases:  (1)  design  and  build  a  manipulator,  (2)  attach  it  to  a  base  spacecraft  simulator, 
(3)  characterize  its  kinematic  and  dynamic  behavior,  and  (4)  control  the  manipulator  to 
perform  a  value-added  task.  The  first  SRL  multi-link  manipulator  program  was  a  four- 
link  manipulator  constructed  in  2012  (Figure  8). 
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Figure  8.  The  First  SRL  Multi-link  Manipulator.  Source:  [20]. 

In  this  same  year,  the  16  m"  polished  granite  monolith  was  installed  in  the 
laboratory,  providing  additional  maneuver  space  over  the  legacy  9  m“  monolith.  The 
table-top  method  experimentation  allows  for  a  spacecraft/manipulator  system  that 
operates  in  three  degrees  of  freedom  (translation  in  the  x  and  y  directions  of  the  Cartesian 
plane  and  rotation  about  the  z  axis,  which  is  perpendicular  to  the  table.  A  four-joint 
robotic  manipulator  system  is  desirable  because  it  offers  four  degrees  of  freedom 
(4DOF).  Despite  the  dynamic  coupling,  a  4DOF  manipulator  operating  in  a  3DOF 
experimental  environment  enables  the  base  to  remain  stationary  under  the  right 
circumstances.  Such  kinematic  redundancy  is  convenient  if,  for  example,  the  host 
spacecraft  has  a  camera  that  needs  to  observe  the  end  effector  working. 

The  first-generation  manipulator  system  was  designed  to  attach  to  the  lab’s  third 
generator  Floating  Spacecraft  Simulator  (FSS),  the  tall  rectangular  prism  in  Figure  8.  As 
in  nearly  all  manipulators,  the  links  depended  upon  the  base  spacecraft  for  power  and 
computation,  requiring  electrical  wiring  to  be  strung  throughout  the  link.  Once 
constructed,  the  manipulator  could  not  be  reconfigured,  limiting  the  number  and  type  of 
table-top  experimental  problems  to  which  the  system  could  be  applied. 

The  solution  to  this  limitation  was  to  design  and  build  a  modular  link  that 
contained  all  of  the  power,  hardware,  and  software  within  its  own  structure  (Figure  9). 
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These  independent  links  are  compatible  with  the  new  fourth-generation  FSS  (Figure  10), 
which  was  designed  to  allow  manipulator  mountings  on  each  of  its  four  faces.  In  the 
current  configuration,  the  links  depend  upon  the  FSS  only  for  the  compressed  air  that 
flows  through  a  series  of  detachable  air  hoses  to  the  link  air  pads. 
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Figure  9.  Complete  Link  Assembly.  Source:  NPS  SRL. 


Four  modular,  independent  links  can  be  reconfigured  and  mounted  on  the  FSS  in 
any  number  of  ways.  For  example,  all  four  links  could  be  attached  into  one  serial 
manipulator  as  in  the  current  investigation,  but  two  manipulators  could  just  as  easily  be 
affixed  to  two  adjacent  (or  two  opposite)  sides  for  experimentation  with  two  cooperative 
(or  two  independent)  manipulators  such  as  EFFORTS-2,  ARFR,  the  Massachusetts 
Institute  of  Technology’s  (MIT)  Experimental  Free-Flying  Robot  [21],  and  the  Tsinghua 
University’s  Humanoid  Free-flying  Space  Robot  [22].  The  two-armed  manipulators 
provide  examples  of  systems  designed  to  have  cooperative  manipulators,  but  like  their 
single  manipulator  counterparts,  they  lack  the  ability  to  be  reconfigured.  The  cooperative 
control  of  both  manipulators  presents  a  challenge  beyond  the  scope  of  this  thesis,  but  is 
recommended  as  a  future  area  of  investigation. 
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Figure  10.  The  Floating  Spacecraft  Simulator  (Without  Reaction  Wheel). 

Source:  NPS  SRL. 

The  kinematics  and  dynamics  of  each  configuration  and  the  on-board  control 
algorithms  would  change  for  different  scenarios,  but  the  hardware  and  the  software  to 
calibrate  the  kinematics  and  dynamics  would  remain  largely  the  same.  An  entirely  new 
manipulator  would  not  need  to  be  built  for  each  series  of  experiments,  which  opens  the 
possibility  of  multiple  investigations  without  the  time-consuming  and  expensive  process 
of  designing,  building,  testing,  and  configuring  a  new  robotic  manipulator  every  time  the 
mission  set  changes.  To  that  end,  the  first  link  of  the  SRL’s  second-generation  spacecraft 
manipulator  was  designed  and  built  by  Dr.  Markus  Wilde  and  Mr.  Daniel  Alvarez  from 
March  2013  to  October  2014  [20].  At  that  time,  the  modular  manipulator  design  (named 
the  Modular  Arm  Robotic  Manipulator,  or  MARSMAN)  was  believed  by  the  researchers 
to  be  the  first  of  its  kind  in  the  field.  Dr.  Josep  Virgili-Llop,  a  National  Research  Council 
post-doctoral  fellow  at  the  NPS  NRL,  and  the  author  continued  work  on  the  project 
beginning  in  June  2014. 

C.  RESEARCH  OBJECTIVES 

The  primary  purpose  of  the  investigation  was  to  achieve  table-top 
experimentation  of  a  workable,  modular,  four-link,  single  manipulator  system  and  to 
calibrate  the  systems  kinematics  and  dynamics.  Although  the  temptation  for  continued 
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design  improvement  was  ever  present,  investigations  into  modified  structural  concepts 
was  limited.  Several  significant  hardware  modifications  were  made,  however,  and  these 
are  explained  in  Chapter  II.  In  the  process,  a  6DOF  kinematics  and  dynamics  calibration 
model  was  developed  and  validated  (in  3DOF)  through  experimentation. 

The  4-link  MARSMAN  configuration  is  believed  to  be  the  first  multi-link 
manipulator  composed  of  modular,  reconfigurable  links.  It  is  also  significant  to  note  that 
the  mass  ratio  of  the  base  to  the  manipulator  (approximately  1.3),  and  the  inertia  ratio  of 
base  to  the  manipulator  (approximately  0.02  when  the  manipulator  is  fully  extended)  are 
significantly  smaller  than  any  of  the  systems  previously  discussed.  These  small  numbers 
are  indicative  of  a  system  with  significant  dynamic  coupling  and  highly  non-linear 
dynamics. 

An  open-source  software  model  named  the  Spacecraft  Robotics  Toolkit  (SPART) 
was  presented  at  the  6th  International  Conference  on  Astrodynamics  Tools  and 
Techniques  (ICATT)  [23],  and  in  an  effort  to  reduce  the  duplication  of  efforts  among 
roboticists  worldwide,  the  software  was  made  publicly  available  via  the  GitHub  open- 
source,  collaborative  development  environment  [24].  To  the  knowledge  of  the  author, 
SPART  is  the  first  open-source  kinematics  and  dynamics  simulator  for  spacecraft 
robotics. 

With  the  four-link  manipulator  complete,  the  investigation  is  ready  to  proceed  in 
any  number  of  directions,  including  the  control  of  the  robotic  manipulator,  the  integration 
of  an  end  effector,  or  the  use  of  the  manipulator  in  capture  and  berthing  scenarios.  A 
more  detailed  discussion  on  the  course  of  future  work  is  offered  in  Chapter  VI: 
Conclusion. 
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II.  DESIGN  EVOLUTION 


This  chapter  recounts  the  evolution  of  the  manipulator  link  design,  highlighting 
the  hardware  and  software  changes  between  the  first  and  subsequent  manipulator  links. 
While  Link  1  was  functional,  there  were  multiple  areas  ripe  for  improvement.  These 
design  upgrades  will  be  discussed  in  further  detail,  but  prior  to  experimentation,  Link  1 
was  upgraded  with  the  same  components  as  Links  2-4.  Thus,  all  four  links  follow  the 
same  design. 

A.  REQUIREMENTS 

The  original  design  requirements  were  found  to  adequately  meet  the  experimental 
goals  of  the  project  but  were  modified  to  employ  Wi-Fi  and  commercial-off-the-shelf 
(COTS)  components.  The  design  requirements  thus  remain  the  same  as  those  outlined  by 
Wilde  and  Alvarez  [20]  with  one  modification  and  one  addition: 

•  Easily  interchangeable  modular  design 

•  No  wires  routed  through  joints 

•  MODIFIED:  Wireless  data  relay  to  base  robotic  vehicle  via  Wi-Fi 

•  On-board  power  supply 

•  On-board  servomotor  and  encoder 

•  On-board  torque  sensor 

•  Highest  accuracy  components  for  reasonable  cost  and  mass 

•  ADDED:  Use  all  commercial-off-the-shelf  (COTS)  components 

B.  MODIFIED  DESIGN 

1.  Retained  Hardware 

The  majority  of  the  hardware  components  of  the  Link  1  were  retained  for 
subsequent  links.  These  components  include  the  3D-printed  link  structure,  the  Harmonic 
Drive  FHA-8C-30-12S17b-E  servomotor  and  the  Harmonic  Drive  DEP-090-09 
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servomotor  driver  [25],  the  FUTEK  TFF400  torque  sensor  [26],  the  Inspired  Energy 
NH254  lithium-ion  battery  [27],  the  Traco  Power  75-241 5 WI  V  DC/DC  converter  [28], 

2.  The  Link  Structure 

The  structure  of  each  manipulator  link  consists  of  five  separate  polycarbonate 
pieces  designed  in  Siemens  NX  computer-aided  design/computer-aided 
manufacturing/computer-aided  engineering  (CAD/CAM/CAE)  software.  Each  piece  was 
created  in  a  part  file  format  (.prt)  and  exported  to  the  printer  as  a  stereo  lithography  file 
(•stl). 

Lesson  Learned 

While  the  NX  software  is  very  sophisticated,  a  freely  available  software  package 
like  the  Windows  3D  Builder  application  can  be  used  to  view  and  manipulate  the 
.stl  files.  In  this  project,  3D  Builder  was  used  to  create  the  link  shoes  (see  Section 
II. 3. e.).  The  default  file  format  for  items  saved  from  3D  builder  is  the  ,3mf format, 
but  objects  can  be  saved  as  .stl  files. 

The  five  pieces  of  the  link  are  the  main  link  structure,  the  outer  plate,  the  motor 
carriage,  the  inner  bracket,  and  the  outer  bracket.  Figure  1 1  shows  the  pieces  as  they 
arrive  from  the  3D  printer.  Figure  12  shows  the  same  pieces  after  the  substrate  has  been 
removed. 
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Figure  11.  Five  Pieces  of  the  Main  Link  Structure  with  Substrate. 


Figure  12.  Five  Pieces  of  the  Main  Link  Structure  without  Substrate. 
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a.  The  FHA-8C-30-12S1 7b-E  Servomotor  and  the  DEP-090-09 
Servomotor  Driver 

The  FHA-8C-30-12S17b-E  harmonic  drive  servomotor  (Figure  13)  is  the  on¬ 
board  actuator  for  each  manipulator  link.  It  responds  to  commands  from  the  DEP-090-09 
servomotor  driver  (Figure  14) — a  driver  chosen  specifically  for  its  compatibility  with  the 
servomotor’s  absolute  encoder.  The  servomotor  sends  its  position  data  back  through  the 
servomotor  driver.  The  driver  calculates  velocity  based  upon  the  position  data  and  notes 
the  amount  of  applied  current  (a  proxy  for  applied  torque).  The  controller  uses  these 
kinematic  data  points  for  each  link. 


Figure  13.  FHA-8C-30-12S17b-E  Servomotor. 
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Figure  14.  DEP-090-09  Servomotor  Driver.  Source:  [25]. 

The  driver  is  programmed  via  the  accompanying  Copley  Motion  Explorer  2 
software  produced  by  the  Copley  Controls  company  [29].  Driver  settings  are  chosen 
through  the  graphic  user  interface  and  can  be  tested  through  an  RS232  serial  line 
connection  between  the  driver  and  the  programming  computer.  Once  settings  are 
satisfactory  for  the  mission  requirements,  they  can  be  saved  and  uploaded  to  the 
driver’s  flash  memory  as  a  proprietary  Copley  Controls  Axis  (.ccx)  file.  For  the  .ccx 
file  used  in  this  project  and  the  chosen  parameter  settings,  see  Appendix  D: 
MARSMAN_Driver_Load.ccx.  Table  1  lists  size,  weight,  power,  and  cost  characteristics 
of  both  the  servomotor  and  the  driver. 


Table  1.  Servomotor  and  Driver  Characteristics.  Sources:  [20],  [25],  [26]. 


Hardware 

Dimensions 

Mass 

Input  Voltage 

Input  Current 

Cost 

Servomotor 

50x49x48. 5mm 

0.5kg 

24V  DC 

3A 

$2,500 

Driver 

196x99x3 1mm 

0.45kg 

24V  DC 

10A 

$700 

Lesson  Learned 

To  maintain  the  memory  of  its  current  location  when  the  primary  power  source  is 
removed,  the  encoder  requires  input  from  an  alternate  power  source.  In  the 
current  design,  this  alternate  power  source  is  a  1.3V  Daytona  battery.  This  very 
important  power  source  was  not  documented  in  the  initial  assembly  literature. 
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b.  The  FUTEK  TFF400  Torque  Sensor 

The  FUTEK  TFF400  torque  sensor  (Figure  15)  attaches  to  the  servomotor  and 
senses  the  amount  of  torque  applied  by  the  servomotor. 


Figure  15.  FUTEK  TFF400  Torque  Sensor,  Isometric  and  Rear  Views. 

Source:  [26] 

As  the  motor  applies  torque  to  the  Link  body’s  outer  plate,  the  mechanical 
twisting  experienced  by  the  face  of  the  torque  sensor  is  converted  into  a  voltage,  which 
flows  into  the  sensor’s  Wheatstone  bridge  circuit  at  the  +Excitation  node  (Figure  16).  The 
current  signal  is  read  at  the  +Signal,  -Signal,  and  -Excitation  nodes  and  those  currents 
pass  to  a  load  cell  amplifier — a  COTS  integrated  circuit  that  is  a  new  element  in  the 
evolved  design — and  then  to  the  Arduino  Due  microprocessor  for  processing  and 
transmission.  It  should  be  noted  that  a  low-level  torque  will  produce  a  reading  that  is 
difficult  to  distinguish  from  the  sensor  noise.  Table  2  lists  size,  weight,  power,  and  cost 
characteristics  of  the  torque  sensor. 
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WC1:  Standard  4-Wire 


Figure  16.  Wiring  Diagram  for  FUTEK's  TFF400  Torque  Sensor.  Source:  [30]. 
Table  2.  Torque  Sensor  Characteristics.  Sources:  [4],  [20]. 


Hardware 

Dimensions 

Mass 

Output  Voltage 

Resolution 

Cost 

Torque  Sensor 

50.8  x  50.2mm 

0.25kg 

3mv/V 

0.003Nm 

$1,040 

c.  The  Inspired  Energy  NH2054HD31  Lithium-Ion  Battery 

The  main  power  source  for  each  link  is  the  Inspired  Energy  NH2054HD31 
(Figure  17).  This  rechargeable,  lithium-ion  battery  has  a  nominal  voltage  and  current  life 
of  14.4V/6.2Ah — an  extremely  capable  battery  for  this  experimentation.  The  battery  and 
its  mounting  remained  the  same  from  the  initial  design,  but  the  manner  in  which  the 
voltage  was  routed  changed  to  provide  the  necessary  24V  to  the  driver  and  servomotor 
and  the  necessary  5V  to  the  Arduino. 
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Figure  17.  The  Inspired  Energy  NH2054  Li-Ion  Battery.  Source:  [27]. 

Lesson  Learned 

The  manufacturer  recommends  an  input  voltage  for  the  Arduino  Due 
microprocessor  of  7-12V  with  6V  and  16V  being  the  absolute  upper  and  lower 
bounds  [31].  Ostensibly,  the  Arduino  Due  seems  compatible  with  the 
NH2054HD31.  However,  the  NH2054HD31  specification  sheet  notes  that  the 
battery  can  charge  up  to  a  maximum  of  16.8V — a  voltage  that  will  melt  the  Due’s 
onboard  voltage  converter  [27].  Temporary  solutions  included  using  slightly 
discharged  batteries  and  utilizing  an  auxiliary  9V  battery.  Ultimately,  a  voltage 
regulator  was  installed  between  the  main  power  source  and  the  Arduino  Due.  The 
discharge  curve  for  this  battery  is  shown  in  Figure  18. 
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Figure  18.  Discharge  Curve  of  14.4V  Li-Ion  Battery.  Source:  [27] 


d.  The  Traco  Power  75-2415WI  V  DC/DC  Converter 

The  Traco  Power  75-24 15WI  V  DC/DC  converter  takes  the  nominal  14.4V  from 
the  main  power  source  and  converts  it  to  the  24V  needed  to  power  the  servomotor  driver 
and  the  servomotor,  which  are  connected  in  series.  It  does  not  provide  any  power  to  the 
torque  sensor  as  indicated  in  Link  l’s  initial  design  electrical  power  architecture  [20]. 
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Figure  19.  Traco  Power  75-2415WI  DC/DC  Converter.  Source:  [28]. 
e.  The  Arduino  Due  Microprocessor 

The  Arduino  Due  is  a  hobby-grade  microprocessor  board  that  takes  advantage  of 
the  C  programming  language  and  Arduino’ s  own  Integrated  Development  Environment 
(IDE)  (Figure  20).  It  is  capable  of  reading  the  analog  torque  sensor  output  signals  and 
converting  them  to  digital  signals,  which  are  then  passed  through  a  compatible  piece  of 
communications  hardware.  In  the  initial  design,  a  compatible  XBee  shield  was  mated 
with  the  Arduino  Due  to  pass  position,  velocity,  and  torque  data.  In  the  evolved  design, 
the  Arduino  Wi-Fi  shield  replaced  the  XBee  shield.  The  Arduino  nominally  runs  on  3.3V 
of  power  but  can  accept  recommended  input  voltages  between  7-12V  [31]. 
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Figure  20.  Arduino  Due  Microcontroller.  Source:  [31]. 


/.  The  LinkSprite  RS232  Shield  and  Serial  Connection  Hardware 

The  LinkSprite  RS232  shield  is  an  Arduino-compatible  shield  that  allows  the 
Arduino  Due  to  transmit  and  receive  data  to/from  the  servomotor  driver  via  serial  cable 
(Figure  21). 


BH 


Figure  21.  LinkSprite  RS232  Shield.  Source:  [32]. 
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g.  The  Dantona  3.6V  Auxiliary  Battery 

This  Dantona  3.6  battery  (Figure  22)  provides  a  bias  voltage  to  the  motor  encoder 
when  the  primary  power  source,  the  14.4V  battery,  is  removed.  Without  a  power  supply, 
the  absolute  encoder  will  not  retain  knowledge  of  its  position. 


Figure  22.  Dantona  3.6V  Battery. 


h.  Air  Delivery  System  Components 

The  FSS  houses  a  tank  of  compressed  air,  which  flows  to  each  of  the  links  via 
0.125"  plastic  tubing.  In  keeping  with  the  modular  design  intent,  each  link  has 
independent  tubing  that  attaches  to  the  adjacent  links  via  interlocking  tubing  connectors. 
A  T-splitter  allows  for  air  flow  to  the  air  bearing  itself  which  connects  to  the  tube  via  a 
brass  pipe  fitting.  The  bearing  is  made  of  a  porous  type  of  carbon  that  allows  the 
compressed  air  to  flow  through  it.  An  adjustable  threaded  ball  stud  attaches  to  the  bottom 
of  the  main  link  structure  and  fits  within  the  socket  of  the  bearing.  A  3-D  printed  cap 
designed  and  printed  in-house  keeps  the  ball  joint  seated  in  the  bearing.  Figure  23  shows 
these  parts. 
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Figure  23.  Air  Delivery  System  Components. 


3.  Hardware  Upgrades 

With  the  goals  of  maximizing  COTS  equipment  to  decrease  link  assembly  time, 
ensure  repeatable  results,  and  improve  the  resolution  of  the  signal  reading  from  the  torque 
sensor,  multiple  significant  hardware  changes  were  implemented.  First,  the  custom 
circuitry  implemented  in  the  original  link  was  replaced  with  a  commercial-off-the-shelf 
(COTS)  load  cell  amplifier.  The  removal  of  the  custom  circuitry  necessitated  the 
installation  of  a  voltage  regulator  between  the  DC/DC  converter  and  the  Arduino 
package.  Second,  the  XBee  radio  transmitter  was  replaced  with  an  Arduino  Wi-Fi  shield 
that  was  compatible  with  the  Arduino  Due  board  and  the  originally-used  RS232  interface 
board.  Third,  the  custom  serial  cable  connecting  the  servomotor  driver  to  the  RS232 
shield  was  replaced  with  functionally  identical  COTS  pieces.  Finally,  several  subtle 
design  improvements  were  incorporated  into  the  design. 

a.  The  SparkFun  Load  Cell  Amplifier-HX711 

To  classify  the  dynamics  of  the  robotic  manipulator,  information  on  applied 
torque  is  essential.  The  previously-discussed  FUTEK  torque  sensor  is  responsible  for 
measuring  the  torques  applied  by  the  motor,  but  the  mechanism  by  which  that  data  is 
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passed  from  the  torque  sensor  (an  analog  signal  from  a  piece  of  hardware)  to  analyzable, 
digital  data  is  an  analog-to-digital  converter  (ADC).  In  this  case,  the  links  employ  a 
Sparkfun  load  cell  amplifier.  The  SparkFun  Load  Cell  Amplifier-HX7 1 1  is  shown  in 
(Figure  24). 

The  crucial  component  of  the  load  cell  amplifier  is  the  HX7 1 1  integrated  circuit,  a 
chip  specifically  designed  to  read  a  signal  from  a  load  cell  (i.e.  the  torque  sensor)  at  high 
precision.  An  open-source  header  file  (HX7 1 1  .h)  and  a  complimentary  C++  source  code 
file  (HX7 1 1  .cpp)  were  available  from  SparkFun  and  were  incorporated  into  the  final 
Arduino  code  (See  Appendix  A:  SparkFun  FIX711  Load  Cell  Amplifier  Open-source 
Header  File  and  Appendix  B:  SparkFun  HX711  Load  Cell  Amplifier  Open-source  C++ 
Source  Code  File). 
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Figure  24.  Both  sides  of  the  SparkFun  Load  Cell  Amplifier.  Source:  [33]. 


To  define  “high  precision,”  it  is  necessary  to  investigate  the  torque  sensor 
specifications,  specifically  how  it  responds  to  varying  voltages.  According  to  its 
specifications,  the  torque  sensor  has  a  rated  output  of  2mV/V  when  the  torque  input  is  at 
a  maximum  (400  in-oz  or  approximately  2.82  Nm)  [26].  With  the  15V  excitation  to  the 
torque  sensor  from  the  DC/DC  converter  in  the  original  design,  the  voltage  difference 
between  the  terminals  of  the  torque  sensor  was  found  to  vary  by  a  very  small  amount, 
only  ±  30mV  [20].  The  intent  of  the  custom  circuit  board  in  that  design  was  to  boost  the 
strength  of  the  torque  sensor  output,  ideally  to  the  3.3V  accepted  by  the  Arduino’s  analog 
input  port  [20].  The  Arduino  itself  can  only  serve  as  a  10-bit  analog-to-digital  converter 
(ADC).  Thus,  each  torque  sensor  reading  can  be  represented  by  one  of  ten  integers  from 
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zero  to  2 10  (or  1,024).  While  this  method  was  workable,  it  only  provided  a  best-case 
resolution  of3.3V/21"  =3.2mV,  which  is  a  poor  resolution  in  relation  to  the  ±30mV 
measured  variation.  Implementing  a  COTS  load  cell  amplifier  eliminated  the  need  for 
custom  parts  while  allowing  for  greater  resolution  of  the  torque  signal. 

The  Spark  Fun  load  cell  amplifier  uses  the  HX7 1 1  24-bit  analog-to-digital 
converter  (ADC)  to  transform  the  torque  sensor’s  analog  electrical  signal  output  into  a 
digital  signal  that  can  be  processed  by  the  Arduino  stack  and  analyzed  within 
Simulink/M ATTAR.  Since  the  HX711  has  a  capacity  of  24  bits,  it  can  represent  224  (or 
16,777,216)  integers.  The  nominal  middle  of  the  HX711’s  range  is  8,388,608.  With  zero 
load  the  torque  sensor  read  in  the  range  of  8,374,000  with  fluctuations  due  to  sensor 
noises — a  reading  reasonably  close  to  the  nominal  middle.  With  24  bits,  the  best-case 
resolution  becomes 3. 3V/224  wl.97xl0~4mV,  which  is  a  vast  improvement  over  using 
the  Arduino ’s  ADC. 

The  second  step  in  the  calibration  of  the  data  was  to  calculate  the  analog  input  to 
the  torque  sensor.  With  the  Arduino  putting  out  5V,  the  analog  voltage  leaving  the 
HX711  (pin  3,  Analog  Voltage  Drain  or  “AVDD”),  can  be  calculated  via  the  equation 
provided  in  the  system  diagram,  Equation  1 . 


AVDD 


VBG(R\  +  R2) 
R\ 


(1) 


In  the  above  equation,  the  bandgap  voltage  (VBG)  is  the  reference  bypass  analog 
output  voltage  (pin  6),  a  fixed  value  of  1.25V.  R1  and  R2  are  the  resistor  values  within 
the  circuit,  8.2 kQ.  and  20  kQ ,  respectively  [34],  Note  that  the  R1  and  R2  values  listed  in 
the  product  diagram  are  reversed — a  mistake  that  caused  some  confusion  during  initial 
measurements.  When  calculated  as  listed  in  the  diagram,  the  analog  power  supply  is 
found  to  be  1.78V — outside  the  nominal  range  of  the  HX71 1  (2.2  to  -5.5V).  The  voltage 
was  measured  at  the  input  to  the  torque  sensor  and  found  by  voltmeter  to  be  4.33V,  very 
close  to  the  mathematically  predicted  AVDD  of  4.30V  when  Rl=  8.2  kQ  and  R2=20kQ . 
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On  the  HX71 1,  Channel  A  can  be  programmed  with  a  gain  of  128  or  64  [34].  The 
higher  gain  value  was  chosen  and  implemented  via  the  Arduino  code  to  allow  for  a 
higher  resolution  in  the  sensor  output.  The  system  specifications  state  that  a  gain  of  128 
and  an  AVDD  of  5V,  the  corresponding  voltage  output  is  expected  to  be  within  the  range 
of  +-20mV  [34],  In  the  case  of  the  torque  sensor,  the  AVDD  value  is  less  than  5V,  so  the 
actual  range  can  be  calculated  by  Equation  2  to  be  ±  17mV. 


Nominal  Range  Actual  Range 

Nominal  AVDD  Actual  AVDD  (2) 

Finally,  the  torque  sensor  itself  has  a  calibrated  output  of  1.9873mV/V  at  400  in- 
oz  (2.8246Nm).  With  4.3V  actually  applied  to  the  sensor,  the  torque  constant  ( k )  as  a 
function  of  voltage  can  be  calculated.  By  Equation  3,  every  mV  applied  results  in  .33Nm 
of  torque.  This  mathematical  model  was  implemented  in  the  Arduino  code.  The  results  of 
the  code  were  verified  by  measuring  the  applied  torque  with  an  analog  torque  watch  and 
the  voltage  output  with  a  voltmeter. 


k 


2.8246 Am 

1.9873  Am/m  Vx  4.3V 


.33  Am/ mV 


(3) 


b.  The  Arduino  Wi-Fi  Shield 

The  Arduino  Wi-Fi  shield  replaced  the  Xbee  wireless  communication  shield  from 
the  original  design  for  a  number  of  reasons.  The  Xbee  shield  was  capable  of  adequate 
transmission  rates  and  ranges  for  the  application,  but  it  depended  upon  the  Zigbee 
protocol,  a  more  obscure  cousin  of  Wi-Fi.  Since  the  FSS  and  the  VICON  system  use 
Wi-Fi,  an  Arduino-compatible  Wi-Fi  shield  seemed  like  the  obvious  choice  for 
implementation  although  Bluetooth  was  briefly  considered.  Second,  communication  with 
the  on-board  XBee  required  attaching  an  XBee  transmitter  to  the  USB  port  of  a  command 
laptop.  A  Wi-Fi  shield  is  capable  of  interfacing  directly  with  the  command  laptop’s 
organic  Wi-Fi  capability,  and  although  an  external  router  was  used  in  this  thesis,  a  more 
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capable  microprocessor  (for  example,  a  Raspberry  Pi)  could  be  used  to  create  an  ad  hoc 
Wi-Fi  network.  Finally,  due  to  a  lack  of  on-hand  supplies  and  the  discontinuance  of  the 
Xbee  shield,  determining  a  replacement  became  necessary.  In  an  ironical  twist,  the 
Arduino  Wi-Fi  shield  that  was  chose  as  the  successor  to  the  Xbee  shield  was  itself 
discontinued  by  Arduino  during  the  evolutionary  design.  A  top  view  of  the  Arduino  Wi¬ 
Fi  shield  is  shown  in  Figure  25. 


Figure  25.  Arduino  Wi-Fi  Shield.  Source:  [35]. 


The  final  limitation  of  the  Arduino  Wi-Fi  shield  is  that  it  is  no  longer  being 
manufactured;  Arduino  has  discontinued  the  product  line  and  is  no  longer  updating  the 
Wi-Fi  Shield  webpage.  Thus,  as  efforts  were  being  made  to  integrate  the  shield  into  the 
existing  configuration,  the  hardware  itself  was  already  obsolete.  Future  work  must 
explore  the  integration  of  a  replacement  for  Arduino  Wi-Fi  shield,  such  as  the  Raspberry 
Pi  microcontroller  with  Wi-Fi  dongle  (more  on  that  in  Chapter  VI). 
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c.  The  RS232  Shield  and  Serial  Cable  Connection 

The  RS232  shield  requires  a  gender  changer  to  connect  the  serial  cable  to  the 
servomotor.  Both  ends  of  the  serial  cable  are  capped  with  RJ12  connectors.  The 
introduction  of  the  COTS  gender  changer  and  a  new  serial  cable  that  follows  the  U.S. 
Bell  System  coloring  scheme  eliminated  the  need  for  tedious  customization  of  a  serial 
connector  and  the  use  of  a  null  modem. 

Lesson  Learned 

Not  all  serial  cables  are  wired  the  same.  The  cables  used  in  this  project  use  the 
U.S.  Bell  System  Coloring  scheme.  As  viewed  from  the  front  of  the  RJ12 
connector,  the  colors  of  the  wires  are  ( from  left  to  right)  white,  black,  red,  green, 
yellow,  blue.  When  both  ends  of  the  cable  are  placed  side-by-side,  they  display  the 
same  pattern  (the”  standard”  configuration ).  If  the  color  patterns  mirror  one 
another  ( the  “reversed”  configuration ),  the  cable  still  can  be  used,  but  a  null 
modem  needs  to  be  inserted  between  the  RJ12  connector  and  the  RS232  port  to 
flip  the  transmit  and  receive  signals. 

d.  Voltage  Regulator 

To  keep  the  battery’s  output  voltage  in  a  safe  operating  range  for  the  Arduino,  a 
voltage  regulator  was  installed  to  convert  the  16.8V  maximum  voltage  to  a  5V  input  for 
the  microprocessor  (Figure  26). 
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Figure  26.  Voltage  Regulator.  Adapted  from  [36]. 


e.  Miscellaneous  Modifications  and  Improvements 

In  addition  to  the  significant  component  changes,  several  more  subtle  changes 
were  included  in  the  design.  Mounting  hole  placements  were  modified  to  better 
accommodate  placement  of  the  microcontroller,  a  Velcro  strap  was  added  to  prevent 
rattling  of  the  battery  during  testing,  and  bolt  usage  was  standardized  to  the  greatest 
extent  possible.  Apart  from  the  bolts  required  to  mount  the  torque  sensor,  all  other  bolts 
are  button  hexagonal  head,  4mm  diameter. 

Finally,  a  link  with  an  attached  air  bearing  presented  a  problem:  the  link  could  not 
stand  because  the  air  bearing  was  jutting  out  the  bottom  of  the  structure.  Initially, 
cardboard  boxes  were  used  to  prop  up  the  completed  links,  but  these  were  fairly  unstable 
(and  unsophisticated).  To  address  that  shortcoming,  the  freely  available  Microsoft  3D 
Builder  application  was  used  to  model  “shoes”  for  the  links.  The  shoes  (Figure  27)  lift  the 
link  off  of  the  table  high  enough  so  that  the  dangling  air  bearing  does  not  touch.  As  a  side 
note,  the  3D  Builder  software  was  also  useful  for  viewing  the  link  components  when 
licenses  with  the  more  advanced  Computer  Aided  Drafting  Software  became 
problematic. 
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Figure  27.  “Shoe”  Developed  Using  the  Free  Microsoft  3D  Builder  Application 
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III.  ASSEMBLY  DOCUMENTATION 


A.  INTENT  OF  THE  ASSEMBLY  DOCUMENTATION  SECTION 

The  intent  of  this  section  is  to  document  the  assembly  of  a  link  to  a  sufficient 
degree  that  future  researchers  wishing  to  replicate  the  experimental  campaign,  evolve  the 
present  design,  or  develop  an  entirely  different  robotic  manipulator  design  have  a  well- 
documented,  successful  design  from  which  to  begin  their  work.  Although  assembly 
documentation  exists  prior  to  this,  given  the  hardware  upgrades,  a  newer,  more  thorough 
documentation  is  necessary.  This  documentation  additionally  includes  thorough  labeling 
of  part  names,  wiring  diagrams  that  will  prevent  future  agonizing  over  pin-out  schematics 
and  hardware  interfaces,  an  introduction  to  the  driver  programming  software  interface, 
and  an  in-depth  discussion  of  assembly  methodologies  and  lessons  learned.  In  truth,  no 
two  links  were  constructed  in  exactly  the  same  sequence.  However,  by  the  completion  of 
Link  4,  a  best-practice  approach  was  established. 

At  the  outset  of  the  construction  of  each  Link,  the  idea  to  transform  the  side  walls 
of  the  link  structure  into  slide-out  walls  resurfaced.  Walls  that  could  slide  out  would 
avoid  the  difficulties  of  fitting  large  hands  in  small  places  and  trying  to  tighten  nuts  onto 
inconveniently  located  bolts.  The  sliding  wall  modification  was  never  attempted  for  two 
reasons.  First,  in  the  interest  of  research  focus,  a  time-consuming  diversion  into  CAD 
modifications  was  not  desirable.  Second,  and  more  importantly,  it  was  desirable  to  keep 
all  of  the  links  as  structurally  similar  as  possible  to  simplify  the  initial  assessment  of  the 
system  kinematics  and  dynamics.  It  is  possible  that  sliding  walls  would  have  diminished 
the  rigid-body  behavior  of  the  manipulator  link. 

Previous  documentation  of  the  link  assembly  favored  the  approach  of  first 
mounting  the  hardware  to  the  structure  and  wiring  throughout  the  process.  I  suggest  that  a 
simpler  method  is  to  complete  all  of  the  subsystem  wiring  first.  Broadly,  the  manipulator 
link  can  be  viewed  as  four  subassemblies:  the  torque  sensor/servomotor  subassembly,  the 
circuit  board  subassembly,  the  power  subassembly,  and  the  servomotor  driver  (which 
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simply  mounts  to  the  wall  of  the  main  link  structure).  Thus,  the  documentation  of  the 
assembly  begins  not  with  the  link  structure  but  with  the  preparatory  component  wiring. 

B.  BUILDING  A  LINK 

1.  The  Servomotor  Cable  Connectors 

The  servomotor  has  two  wiring  outputs,  the  motor  leads  (top  cable  in  Figure  28) 
and  the  encoder  cable  (bottom  cable  in  Figure  28).  Neither  of  these  two  cables  conies 
with  a  connector  that  is  compatible  with  the  servomotor  driver.  Figure  32  shows  the 
motor  with  stock  connectors;  the  motor  leads  connector  is  the  white  connector,  and  the 
encoder  cable  connector  is  the  black  connector. 


(28) 


Figure  28.  Servomotor  Cabling  Schematic.  Source:  [25]. 
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Figure  29.  Servomotor  with  Stock  Connectors. 

The  motor  leads  are  initially  attached  to  the  six-pin  connector  shown  in  Figure  33. 
The  red,  white,  and  black  wires  correspond  to  the  power  inputs  for  the  three-phase  motor 
(phases  U,  V,  and  W,  respectively).  At  continuous  operation,  the  driver  provides  2.3A, 
2.4A,  and  1.8A  of  current  via  those  three  lines  (U,  V,  and  W,  respectively)  [37].  Pin  4 
attaches  to  the  green/yellow  wire,  which  serves  as  the  ground.  The  designation  of  the 
green/yellow  wire  as  “PE”  in  Figure  30  stands  for  “Protective  Earth,”  a  term  used  by  the 
International  Electrotechnical  Commission  [38]. 
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Connector  model:  350715-1 

Pin  model:  3506901  (E:  770210-1) 

Manufactured  by  AMP 


Pin  No. 

Color 

Motor  lead 

1 

Red 

Motor  phase-U 

2 

White 

Motor  phase-V 

3 

Black 

Motor  phase-W 

4 

Green/yellow 

PE 

5 

- 

- 

6 

- 

- 

Figure  30.  Motor  Lead  Connector  Layout  (Units  in  mm).  Source:  [39]. 


The  stock  connector  is  removed  and  replaced  with  a  4-position  J2  connector  (that 
is,  it  connects  to  the  J2  port  on  the  servomotor  driver).  This  connector,  along  with  the 
servomotor  encoder  cable  connector  (discussed  in  the  next  section)  comes  in  a  connector 
kit  available  from  Copley  Controls  Corporation  (kit  name:  AEP-CK,  AMP-CK,  part 
number:  84-00147-000  Rev  A).  The  new  motor  lead  connector’s  serial  number  and 
nomenclature  are  “57-00605-000:  Plug,  RoHS,  Euro-Style,  4  position,  5.08  mm.”  The 
colored  wires  are  attached  to  the  new  motor  lead  connector  in  accordance  with  Figure  3 1 . 
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Figure  3 1 .  Motor  Cable  Wiring  with  4-Position  J2  Connector. 


2.  The  Encoder  Cable  Connector 

The  encoder  cable  connects  to  the  motor  driver  and  reads  position  data.  From 
position  data,  velocity  data  is  derived  on-board  the  driver.  The  pinout  of  the  stock 
connector  and  corresponding  descriptions  of  the  pins  is  shown  in  Figure  32. 
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(13) 


1A 


Connector  model:  1  -1 903130-4 
Pin  model:  190311 6-2  or  190311 7-2 
Manufactured  by  AMP 


Connector  pin  layout 

♦  Encoder  cable  (US  200) 


Pin  No. 

Color 

Signal 

Remarks 

1A 

White 

Vcc 

Power  supply  input  +5V 

IB 

Black 

GND  (Vcc) 

Power  supply  input  0V 
(GND) 

2A 

Blue 

SD+ 

Serial  signal  differential 
output  (+) 

2B 

Purple 

SD- 

Serial  signal  differential 
output  (-) 

3A 

- 

No 

connection 

- 

3B 

Shield 

FG 

Frame  Ground 

4A 

Orange 

Vbat 

Battery  + 

4B 

Brown 

GND  (bat) 

Battery  -  (GND) 

Figure  32.  Pinout  of  Encoder  Cable  with  Stock  Connector.  Source:  [39]. 

The  20-pin  J3  feedback  connector  (57-00608-000:  Connector,  RoHS,  20  position, 
Mini-D,  solder  cup)  replaces  the  stock  connector.  The  plastic  shell  (47-00130-000: 
Backshell,  RoHS,  plastic,  20  position,  Mini-D)  encases  the  new  20-pin  connector.  Figure 
33  shows  a  picture  of  the  new  connector  pieces  alongside  the  encoder  cable. 
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Servomotor 


Figure  33.  Replacement  Connector  Pieces  Alongside  the  Encoder  Cable. 


The  pinout  diagram  for  the  20-pin  connector  is  shown  in  Figure  34.  Notice  that 
the  brown  and  orange  wires  (the  auxiliary  battery  negative  and  positive  connections, 
respectively)  are  not  connected  to  a  pin.  These  wires  attach  to  a  separate  power  source,  a 
3.6V  Dantona  battery,  as  shown  in  Figure  35.  This  3.6  battery  provides  power  to  the 
motor  encoder  when  the  primary  power  source,  the  14.4V  battery,  is  removed.  Because  it 
is  an  absolute  encoder,  the  encoder  will  not  retain  knowledge  of  its  position  without  a 
power  supply. 


41 


»  □ 

□ 

□ 

□  □ 

19 

12 

□ 

□ 

□  □ 

rn  20 

B 

El 

□ 

□  □ 

9 

2 

□ 

□ 

□ 

*  22  gage 
+  26  gage 

For  color  codes,  reference  Figure  32.  Gold  squares  represent  unused  pins. 

Figure  34.  Pinout  of  Servomotor  Encoder  Cable  with  20-Pin  Connector. 


Figure  35.  Encoder  Cable  with  20-Pin  Connector  and  Attached  Battery. 

3.  The  Torque  Sensor  Wiring 

As  discussed  in  the  review  of  hardware,  the  FUTEK  Model  TFF400  torque  sensor 
has  a  four-pin  output  receptacle.  The  mating  connector  for  this  receptacle,  the 
FGG.0B.304.CLAD35  type  connector,  is  not  included  as  a  component  of  the  torque 
sensor  but  is  purchased  separately.  Wires  were  soldered  to  the  four-pin  connectors,  and 
the  shell  of  the  connector  reassembled  around  the  pins.  The  .CLAD35  connector  has  a  red 
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dot  which  aligns  with  the  torque  sensor  receptacle.  The  output  wires  were  labeled 
(+EXEC,  -EXEC,  +SIG,  -SIG)  to  avoid  confusion  during  their  connection  to  the  load  cell 
amplifier  connections  of  the  same  names.  Figure  36  shows  the  torque  sensor  with  mated 
.CLAD35  connector.  Although  no  standard  wiring  scheme  was  used  from  link  to  link,  in 
the  image  below,  white  wires  attach  to  the  load  cell  amplifier,  and  the  green  wires  will 
eventually  connect  to  the  Arduino  assembly. 

Lesson  Learned 

The  red  dot  on  the  shell  of  the  .CLAD35  connector  swivels  about  the  four -pin 
connector  itself;  it  is  not  fixed  to  the  four-pin  connector.  Thus,  an  alignment  of  the 
connector’ s  red  dot  with  the  torque  sensor’s  red  dot  does  not  necessarily  mean 
that  the  intended  wires  are  aligned  with  the  intended  receptacles.  Extra  care 
should  be  taken  to  ensure  that  the  output  of  each  wire  is  known  so  that  it  can  be 
connected  to  the  appropriate  position  on  the  load  cell  amplifier.  In  one  instance 
where  the  lead  wires  were  mislabeled  coming  out  of  the  receptacle,  no  negative 
effects  on  the  hardware  were  noted. 


Figure  36.  Torque  Sensor  Wired  to  the  Load  Cell  Amplifier. 
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4. 


Battery  Connector  and  DC/DC  Converter 


From  the  nominally  14.4V  Li-ion  battery,  power  flows  according  to  the  block 
diagram  in  Figure  37.  The  DC/DC  converter  connects  to  the  power  and  ground  outputs  of 
the  battery  terminal.  The  driver  and  motor  require  24V,  so  the  voltage  is  up-converted 
and  passed  to  the  driver’s  J1  connection  portal  via  a  three-pin  connector  similar  to  the 
one  used  on  the  motor  cable  wiring  (from  the  same  connector  kit,  part  57-00604-000: 
Plug,  RoHS,  Euro-Style,  3  position,  5.08  mm).  Note  that  only  the  voltage  and  ground 
pins  are  used  in  the  three-pin  connector. 

While  the  driver/motor  assembly  requires  24V,  the  voltage  going  into  the  Arduino 
assembly  needs  down-converted.  This  is  managed  via  a  Low  Dropout  regulator  (LDO) 
that  outputs  the  necessary  5V.  The  LDO’s  input  of  14.4V  (nominal)  comes  directly  from 
the  battery.  Figure  37  shows  a  block  diagram  of  the  electrical  power  architecture. 


14.4  V 
3.2  A 
44.8  W 
5.8  A  (max) 
83.2  W  (max) 


14  V  24  V 

2.9  A  1.7  A 

40.8  W  40.8  W 


4.0  W  (max)  4  W  (max) 


Figure  37.  Block  Diagram  of  the  Electrical  Power  Architecture. 


5.  The  Arduino  Subassembly 

The  Arduino  subassembly  consists  of  the  Arduino  Due  microcontroller,  the 
Arduino  Wi-Fi  Shield,  and  the  RS232  shield.  The  Due  uses  a  universal  asynchronous 
receiver/transmitter  (UART)  microchip  to  transmit  data  via  the  transistor-transistor  logic 
(TTL)  method,  which  for  the  Due  remains  at  a  voltage  level  between  zero  and  5V  [40]. 
The  RS232  shield  uses  the  power  from  the  Due  and  converts  the  data  from  TTL  to  the 
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Recommended  Standard  232  (RS232)  fonnat,  which  is  compatible  with  the  driver.  From 
a  hardware  perspective,  the  components  are  simply  stacked  on  top  of  one  another:  the 
Wi-Fi  shield  plugs  into  the  Due,  and  the  RS232  shield  plugs  into  the  Wi-Fi  shield.  The 
RS232  shield  connects  to  the  motor  driver  via  a  serial  cable.  As  noted  previously,  the 
proper  serial  cable  (i.e.,  a  serial  cable  that  uses  the  standard  U.S.  Bell  System  coloring 
scheme)  eliminates  the  need  for  a  null  modem  but  requires  a  gender  changer  to  align  the 
pins.  Jumper  wires  are  used  to  connect  the  Due’s  transmit  and  receive  ports  to  the 
RS232’s  transmit  and  receive  ports.  Given  the  capability  of  the  Due,  different 
transmit/receive  port  combinations  are  possible. 

After  examining  the  Due  pinout  diagram  and  experimenting  with  different 
combinations,  two  successful  combinations  are  offered  here.  Thus,  even  with  a  faulty 
board  or  inexplicable  malfunction,  one  combination  or  the  other  should  allow  for  a  viable 
solution.  In  the  first  successful  case,  the  Due’s  port  16  (Tx)  is  connected  to  the  RS232 
shield’s  digital  port  3,  and  the  Due’s  port  17  (Rx)  is  connected  to  the  RS232  shield’s 
digital  port  2.  This  configuration  works  when  the  jumper  connectors  on  the  RS232  shield 
connect  J2  and  J3  in  the  D2  row  and  J1  and  J2  in  the  D3  row.  In  the  second  case,  the 
Due’s  ports  17  (Rx)  and  16  (Tx)  are  connected  to  the  RS232  shield’s  digital  ports  5  and 
6,  respectively.  This  configuration  requires  that  the  jumper  connectors  on  the  RS232 
shield  connect  J2  and  J3  in  the  D5  row  and  J1  and  J2  in  the  D6  row.  The  connection  of 
the  Arduino  assembly  to  its  power  supply  and  to  the  load  cell  amplifier  is  explained  in 
section  5  with  the  discussion  of  the  Arduino  subassembly  mounting. 

6.  Mounting  the  Components  to  the  Structure 

With  the  majority  of  the  wiring  completed,  the  subassemblies  can  be  joined  to  the 
outer  structure.  Mounting  the  parts  may  seem  a  straightforward  task,  but  the  order  in 
which  they  are  mounted  is  important.  Through  the  experience  of  building  multiple  links, 
an  order  for  assembling  the  components  was  decided  upon  that  allowed  for  necessary 
component  testing  while  avoiding  self-interference.  It  is,  for  example,  not  advisable  to 
mount  the  Arduino  assembly  until  near  the  end  of  the  assembly  because  trouble-shooting 
the  power  subsystem  may  be  necessary,  and  accessing  the  DC/DC  converter  or  battery 
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mount  is  difficult  if  the  Arduino  assembly  is  mounted.  The  procedure  outlined  below 
incorporates  similar  lessons  learned. 

First,  the  torque  sensor  is  screwed  to  the  motor  carriage  with  a  piece  of  rubber 
matting  in  between  them.  The  rubber  matting  was  a  carry-over  from  the  initial  design  and 
is  not  strictly  necessary  for  the  link  function.  The  rubber  does,  however,  help  to  seat  the 
torque  sensor  snugly  on  the  motor  carriage  and  may  possibly  reduce  the  transfer  of  the 
motor’s  vibration  to  the  torque  sensor  (Figure  38).  The  pin  connector  with  completed 
wiring  (including  its  attachment  to  the  load  cell  amplifier)  is  inserted. 


Figure  38.  Torque  Sensor  Mounted  on  the  Motor  Carriage. 


Next,  the  servomotor  is  mounted  to  the  outer  plate  (Figure  39).  The  servomotor 
cables  will  not  run  through  the  slot  in  the  outer  plate  but  through  the  slot  in  the  link 
structure. 
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Figure  39.  Servomotor  Mounted  to  Outer  Plate 

The  motor  carriage  with  affixed  torque  sensor  is  slid  down  over  the  servomotor 
itself,  fitting  snugly  in  place  (Figure  40). 


Figure  40.  Mated  Torque  Sensor  and  Servomotor 

Two  circular  bearings  (Figure  41)  are  added,  one  to  the  axel  on  the  bottom  of  the 
outer  plate  and  a  second  on  the  axel  of  the  outer  bracket.  The  inner  bracket  is  screwed  to 
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the  top  of  the  torque  sensor.  Once  affixed  to  the  outer  plate,  the  outer  bracket’s  bearing 
fits  inside  of  the  circular  hole  on  the  top  of  the  inner  bracket.  Figure  42  shows  the 
completed  joint  assembly.  The  cables  are  run  through  the  opening  in  the  link  structure, 
and  the  inner  bracket  is  bolted  to  the  link  structure. 


Figure  41.  Circular  Bearings  for  Use  on  Outer  Plate  and  Outer  Bracket. 


Figure  42.  The  Completed  Joint  Assembly. 


With  the  joint  assembly  complete,  the  servomotor  driver  can  be  bolted  to  the 


sidewall  of  the  link  structure.  It  will  be  programmed  when  the  power  supply  is 
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operational.  The  battery  connector  is  bolted  to  the  floor  of  the  link  structure,  and  the 
DC/DC  converter  is  mounted  to  the  link  structure’s  wall  opposite  the  servomotor  driver. 
The  three-pin  power  connector  from  the  DC/DC  converter  is  connected  to  the  J1 
receptacle  of  the  servomotor  driver,  the  motor  cable’s  four-position  connector  is  plugged 
into  the  driver’s  J2  receptacle,  and  the  servomotor  encoder  with  20-position  connector  is 
plugged  into  the  driver’s  J3  receptacle.  The  J5  receptacle  is  used  for  the  connection  of  the 
serial  cable  to  the  RS232  shield.  The  J4  and  J6  ports  are  not  currently  used.  Figure  43 
shows  a  diagram  of  the  driver  connection  ports,  and  Figure  44  shows  the  link  assembly  as 
it  looks  at  this  stage  in  the  construction. 
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34:  CONTROL 


34:  CONTROI 


34  SIGNALS 

PIN 

Frame  Ground 

1 

Signal  Ground 

2 

Enadte  GP1  (INI] 

3 

G PI  (IN2) 

4 

GPt  (IN 3) 

s 

GPI  (1N4) 

6 

GPt  (INS) 

7 

GPt  (IN6) 

8 

NS  (IN 7) 

9 

MS  (OUTJ) 

10 

(COHN  A] 

11 

(COMM  B) 

12 

GPt  (CXJTl  t ) 

13 

33:  FEEDBACK 

33  SIGNALS 

PIN 

Frame  Ground 

1 

Signal  Ground 

2 

♦5  Vdc  9  400  mA  Output 

3 

Encoder  A 

4 

Encoder  /A 

s 

Encoder  6 

6 

Encoder /B 

7 

Encoder  X 

8 

Encoder  /X 

9 

Encoder  S 

10 

31:  POWER 

31  SIGNALS 

PIN 

MV  COM 

1 

♦  MV 

2 

HVAUX 

3 

31  CABLE  CONNECTOR: 

3  position  5.08  mm  Euro-Style  plug 
Copley:  57-00465-000 
PCD:  ELFP03210 

Rio  31249103 
Weco:  121-A-111/03 


PIN 

>4  SIGNALS 

14 

GPt  (OUT2^| 

IS 

GPI  (OUT2-) 

16 

Multi-mode  Encoder  A 

17 

Multi-mode  Encoder /A 

18 

Multi-mode  Encoder  B 

19 

Multimode  Encoder /B 

20 

Multi-mode  Encoder  X 

21 

Multi- mode  Encoder /X 

22 

♦  S  Vdc  O  400  mA  Output 

23 

Signal  Ground 

24 

|AIN*) 

25 

IA1N-] 

26 

GPt  [OUT1-] 

33:  FEEDBACK 

PIN 

33  SIGNALS 

11 

Hal  U 

12 

Hal  V 

13 

Hal  W 

14 

(INS)  Motemp 

IS 

Signal  Ground 

16 

Analog  Sm(  ♦  ) 

17 

Analog  Sm<-) 

18 

Analog  Co%(  ♦ ) 

19 

Analog  Co*<-) 

20 

Encoder  /S 

34  CABLE  CONNECTOR: 

Solder  Cup,  26  position  male, 
1.27  mm  pitch 

Cable:  26  conductor,  shielded 
Standard  with  Snap  lodes 
3H:  10126-3000  VE  connector 
3M:  10326-52 F0 -008  backshe* 
Rugged  with  Screw-locks 
Mote*:  54306-2619  connector 
Mole*:  54331-0261  badcshei 


Note:  Molded  able  assem¬ 
blies  are  a  variable  tor  13  b  M 
Seep.  10  tot  cable  colon. 


33  CABLE  CONNECTOR: 

Solder  Cup,  20  position  male, 
1.27  mm  pRch 

Cable:  20  conductor,  shielded 
Standard  w*h  Snap  locks 
3M:  101 20-3000 VE  connector 
3M:  10320-52F0008  backshel 
Rugged  with  Screw-locks 
Mole*:  54306-2019  connector 
Motex:  54331-0201  backshel 


32:  MOTOR 


PIN 

32  SIGNALS 

1 

Frame  Gnd 

2 

Motor  U 

3 

Motor  V 

4 

Motor  W 

32  CABLE  CONNECTOR: 

4  position  5.06  mm  Euro-Style  plug 
Copley:  57-00466-000 
PCD:  ELFP04210 

Rla:  31249104 

Weco:  121-A-111/04 


Figure  43.  Diagram  of  the  Harmonic  Drive  Connection  Ports.  Source:  [41]. 
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Figure  44.  Link  with  Joint  Assembly,  Mounted  Servomotor  Driver 

and  DC/DC  Converter 

At  this  stage,  the  main  power  supply  (the  14.4V  Li-ion  battery)  can  be  inserted 
and  the  servomotor  driver  programmed.  If  the  wiring  is  done  correctly,  the  power 
indicator  light  (the  AMP  light)  on  top  of  the  driver  will  light  solid  green.  Attaching  a 
serial  cable  between  the  J5  (RS232)  port  on  the  driver  and  a  computer  with  the  Copley 
Motion  Explorer  2  (CME2)  software  allows  for  configuring  the  motor  driver.  Figure  45 
shows  the  CME2  graphic  user  interface  (GUI)  upon  initialization  of  the  software. 

As  mentioned  in  the  hardware  section,  the  required  configuration  file  is  a  Copley 
Controls  Axis  (.ccx)  file.  Through  the  GUI  the  user  can  manipulate  system  parameters 
and  create  a  unique  ccx  file,  which  is  saved  using  the  yellow  floppy  disk  icon  near  the  top 
of  the  GUI  window.  For  the  ccx  file  used  in  this  project  and  the  chosen  parameter 
settings,  see  Appendix  D:  MARSMAN  Driver  Load.ccx.  This  particular  configuration  is 
based  upon  the  driver’s  default  configuration  but  contains  adjusted  controller  gains  for 
stiffer  joint  resistance.  To  load  this  file  upon  first  use,  the  user  selects  Amplifier^  Basic 
Setup->Load  ccx  File.  Changes  to  the  ccx  file  are  saved  using  the  floppy  disk  icon  at  the 
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top  of  the  GUI  window.  The  ccx  File  is  saved  to  the  driver’s  internal  flash  memory  using 
the  blue  integrated  circuit  icon  near  the  top  center  of  the  GUI  window. 


^  CME  2  V7.0  (DEP-090-09  Current) 
File  Amplifier  Tools  Help 


s-samas  ■  ■3-anna 


0  Copley  Neighborhood 
G23  Virtual  Amplifier 


(§)  Axis  A 

f  Axis  B 
Axis  C 
Axis  D 


Input  /  Output 


CVM  Control  Program 

nr 

Programmed  Velocity 

1 

_ 1 

Configure  Faults 


(Rotary  Motor 


F12To  Disable 


Figure  45.  Copley  Motion  Explorer  2  (CME2)  Graphic  User  Interface 

upon  Initialization. 


Most  system  troubleshooting  was  done  within  the  Control  Panel  window,  which 
is  accessed  by  clicking  on  the  second  icon  from  the  left  in  the  main  GUI’s  ribbon  bar. 
To  set  the  motor  counter’s  zero  position,  the  motor  first  needs  to  be  disabled.  Disabling 
the  motor  removes  the  applied  voltage,  which  allows  the  user  to  turn  the  joint  assembly 
by  hand  (Figure  46).  This  configuration  takes  the  zero  position  to  be  when  the  link 
is  aligned  with  the  joint,  or  in  other  words,  the  link  rotates  about  the  joint  from 
approximately  -nj 2  to  +  njl . 
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(jj)  Control  Panel 


X 


Status  Monitor 


— 

STO: 

Actual  Current 

SHU A 

o 

•  Motor  Output: 

Not  Active 

o  Hardware  Enabled: 

Enabled 

Actual  Motor  Velocity 

v 

HHU 

. 

•  Software  Enabled: 

Not  Enabled 

a  Positive  Limit: 

u  Negative  Limit: 
u  Software  Units: 

a  Motor  Phase: 

Not  Active 

Not  Active 

Not  Active 

OK 

Actual  Motor  Position 

■pSH 

jpyjE 

Error  Log 

Mode:  Disabled 

Motion  Abort  Input: 

v?  CVM  Control  Program:  Not  Running  Jog 

Home:  Not  Referenced 

Network  Status:  Jog  Speed:  ■ _ 300  ]  rpm 

Gan  Schedufcng:  Not  Active 


Control 


□  Enable  Jog 


Enable 

Set  Zero  Position 

Disable 

1 

dear  Faults 

Reset 

dose 

Figure  46.  CME2  Control  Panel  with  Disabled  Motor  Control. 


Faults  are  most  likely  to  occur  if  the  motor  is  commanded  to  a  position  that  it  is 
not  physically  able  to  reach  (i.e.,  it  thinks  it  needs  to  rotate  further,  but  the  wall  of  the  link 
structure  is  blocking  the  joint  from  turning).  To  prevent  current  overload,  the  controller 
will  implement  its  safety  mechanism  (a  “Latched  Fault”),  which  will  prevent  the 
hardware  from  being  damaged.  If  this  happens,  the  motor  will  be  unresponsive,  and  the 
AMP  light  on  top  of  the  driver  will  blink  red.  When  a  link  is  attached  to  the  CME2 
software,  the  “Clear  Faults”  button  will  reset  the  driver.  During  experimentation  it  is 
cumbersome  to  reattach  the  driver  to  the  CME2  software,  so  removing  and  reinserting  the 
main  battery  will  hard  boot  the  driver.  The  removal  of  the  main  battery  allows  for  manual 
repositioning  of  the  joint. 

With  faults  cleared,  the  software  can  be  enabled  by  clicking  the  “Enable”  button. 
All  indicator  “lights”  in  the  Control  Panel  window  will  show  green  (Figure  47).  Clicking 
the  “Enable  Jog”  box  allows  the  user  to  send  impulse  commands  to  the  motor  in  the 
positive  or  negative  direction  to  ensure  that  the  motor  is  responding  to  the  driver 
commands. 
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Q  Control  Panel 


X 


Status 


© 

STO: 

a  Motor  Output: 

Active 

it  Hardware  Enabled: 

Enabled 

A 

o  Software  Enabled: 

Enabled 

»  Positive  Limit: 

Not  Active 

o  Negative  Limit: 

Not  Active 

Error  Log 

it  Software  Limits: 

Not  Active 

it  Motor  Phase: 

OK 

Motion  Abort  Input: 

»  CVM  Control  Program: 

Not  Running 

«  Home: 

Not  Referenced 

Network  Status: 

~  6am  Scheduling: 

Not  Active 

Control 


Enable 

Set  Zero  Position 

Disable 

dear  Faults 

Monitor 

Actual  Current  v  A 

Actual  Motor  Velocity  v  rpm 

Actual  Motor  Position  ^  counts 

Mode:  Velocity,  Programmed 
Jog 

tog  Speed:  300  rpm 

0  Enable  tog  togNEG  togPOS 

Close 


Figure  47.  CME2  Control  Panel,  Faults  Cleared,  Jog  Enabled. 


With  the  battery  connector,  driver,  and  DC/DC  converter  mounted,  only  the 
mounting  of  the  Arduino  assembly  remains.  The  Arduino  assembly  is  mounted  opposite 
the  battery  and  in  the  current  configuration,  provides  just  enough  clearance  for  the  battery 
to  seat  snugly  into  the  battery  connector.  The  power  and  ground  wires  run  from  the 
battery  connector  to  the  bottom  of  the  DC/DC  converter.  The  power  and  ground  wires  to 
the  Arduino  assembly  attach  directly  to  this  connection  (nominally  14.4V).  As  mentioned 
in  the  hardware  discussion,  the  Arduino  assembly  can  accept  14.4V  directly,  but  the  Li- 
ion  battery  can  hold  a  voltage  of  up  to  16.8V,  which  will  destroy  the  Due’s  on-board 
voltage  regulator.  To  prevent  this  from  happening,  a  separate  voltage  regulator  was 
installed  that  down-converts  the  battery’s  output  from  14.4V  to  5V.  The  5V  output  of  the 
regulator  is  wired  to  the  “+5V”  pin  on  the  RS232  shield.  A  ground  wire  connects  the 
“GND”  pin  directly  to  the  ground  of  the  DC/DC  converter. 

The  load  cell  amplifier  jumper  wires  are  plugged  into  the  Arduino  assembly.  The 
load  cell  amplifier’s  “2.7-5V”  pin  connects  to  the  RS232  shield’s  “+3.3V”  output  pin. 
The  load  cell  amplifier’s  data  (“DAT”)  pin  connects  to  the  digital  input  #8  pin  on  the 
RS232  shield.  The  load  cell  amplifier’s  clock  pin  (“CLK”)  connects  to  the  RS232’s 
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shield’s  #9  digital  input  pin.  A  ground-to-ground  connection  completes  the  circuit.  The 
RS232  shield  is  itself  connected  to  the  servomotor  driver  via  a  serial  connector  and  cable 
plugged  into  the  driver’s  J5  (RS232)  port. 

At  this  point  in  the  construction,  the  hardware  for  the  power  and  data  architectures 
are  in  place.  A  schematic  of  the  power  architecture  is  shown  in  Figure  48,  and  the  data 
architecture  schematic  is  shown  in  Figure  49.  The  data  architecture  will  be  discussed 
more  thoroughly  in  Chapter  IV. 


14.4V 


\ 

f 

LDO 

5V 


GND 


Li-ion  Battery 


GND 


DC  DC 
Converter 


Motor  Cable 


(Ji) 


Figure  48.  Power  Architecture  Schematic 
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Wi-Fi  (UDP) 
via  Router 


Figure  49.  Data  Architecture  Schematic 

The  final  step  in  the  construction  is  the  addition  of  the  air  bearing  assembly. 
Three  pieces  of  plastic  tubing  approximately  a  foot  long  are  attached  to  the  three  ports  on 
the  plastic  T-splitter.  Tubing  connectors  are  attached  to  each  end,  and  the  brass  pipe 
fitting  is  attached  to  the  downward-facing  tube.  The  threaded  end  of  the  pipe  fitting  is 
wrapped  in  Teflon  tape  to  ensure  an  air-tight  seal  and  screwed  into  the  bearing.  The 
threaded  ball  stud  is  inserted  first  through  the  bearing  cap  before  being  attached  to  the 
bottom  slot  of  the  main  link  structure.  While  the  ball  stud  may  be  positioned  anywhere 
along  the  slot,  in  practice,  it  was  positioned  underneath  the  link  center  of  mass.  The  air 
bearing  and  ball  stud  are  saved  until  last  because  once  added,  the  link  can  no  longer  stand 
on  a  flat  surface.  The  H-bracket  “shoe”  was  developed  as  a  holder  for  the  full  link 
assembly.  A  view  of  the  completely  assembled  link  is  shown  in  Figure  50. 


56 


Compressed  air 
Connection  to 
Previous  Link 


Compressed  Air 
Connection  to 
Following  Link 

V- 
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Air  bearing 


Figure  50.  Complete  Link  Assembly.  Source:  NPS  SRL. 


With  all  of  the  hardware  in  place  and  the  driver  configured,  a  more  thorough  look 
at  the  link  operating  concept  and  communications  architecture  are  necessary.  An 
understanding  of  these  components  will  enable  an  understanding  of  the  microcontroller 
code  that  was  implemented  via  the  Arduino  Due.  The  code  itself  proved  to  be  the  most 
challenging  aspect  of  making  the  link  work  successfully.  Following  a  discussion  of  the 
Due’s  code,  an  explanation  of  the  kinematics  and  dynamics  Simulink  model  will  be 
necessary  before  discussing  the  kinematic  and  dynamic  calibration  of  the  manipulator. 
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IV.  DATA  ARCHITECTURE 


A.  OVERVIEW 

This  chapter  explains  the  communications  architecture  of  the  system  given  its 
current  capabilities.  It  outlines  the  concept  of  operations  for  the  manipulator  from  a 
communications  perspective,  explains  the  roles  of  the  on-board  hardware  and  software, 
and  discusses  alternative  methods  of  commanding  the  links.  Finally,  options  for 
architecture  improvements  are  discussed. 

B.  CONCEPT  OF  OPERATIONS 

As  in  previous  testing  with  one  link,  testing  the  four-link  manipulator  involved 
sending  commands  to  the  individual  servomotor  drivers  from  a  laptop  computer  to  the 
links’  on-board  communications  hardware.  Although  the  capability  of  the  FSS  to 
autonomously  control  each  link  was  later  demonstrated,  that  capability  was  not  used  for 
the  calibration  process.  VICON  cameras  tracked  the  motion  of  the  system  via  reflective 
spheres  attached  to  the  base  and  the  outermost  link.  Experimentation  with  the  four- 1  ink 
manipulator  proved  the  concept  that  each  of  the  links  could  be  commanded 
independently.  The  approach  is  conceptually  simple  (as  shown  in  Figure  51),  but  proved 
challenging  in  its  implementation. 
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Figure  51.  Diagram  of  the  Experimental  Setup.  Adapted  from  [42]. 


Because  neither  the  Arduino  Wi-Fi  shield  nor  the  FSS’s  onboard  Wi-Fi  hardware 
are  capable  of  creating  an  ad  hoc  network,  the  communications  architecture  in  its  current 
configuration  depends  upon  an  external  D-Link®  router.  The  router  serves  as  the  hub  for 
all  information  flow  both  to  and  from  the  manipulator  links.  With  the  current  hardware, 
two  methods  of  sending  data  to  and  receiving  data  from  the  links  are  possible.  In  the  first 
method,  which  was  used  for  the  kinematic  and  dynamic  calibration  of  the  manipulator, 
commands  are  sent  from  a  laptop  to  each  of  the  links,  and  the  data  from  each  link  is 
received  by  the  laptop;  the  FSS  neither  sends  nor  receives  data.  In  the  second  method,  the 
commanding  software  is  compiled  and  uploaded  to  the  FSS’s  onboard  computer,  and  the 
FSS  itself  receives  the  telemetry  and  commands  the  links.  Since  the  laptop  control  was 
the  only  method  used  in  this  investigation,  it  is  the  only  one  that  will  concern  us  here. 

C.  DATA  FLOW 

1.  The  D-Link  Interface 

A  discussion  of  the  data  flow  necessarily  begins  with  the  router  and  its  web-based 
interface.  First,  when  the  D-Link  is  powered  on,  it  creates  its  own  network.  The  network 
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is  accessed  by  connecting  to  the  in-house  Wi-Fi  network  named  “srl_dlink.”  The 
interface  is  accessed  through  a  web  browser  at  the  following  URL:  http://192.168.0T/. 

Lesson  Learned 

Considerable  confusion  during  experimentation  resulted  because  the  default 
network  name  and  password  for  the  router  had  never  been  changed.  D-link 
routers  are  a  fairly  common  piece  of  hardware,  and  there  was  another  one  in  the 
building  to  which  the  SRL  hardware  was  connecting  The  router’s  name  was 
reconfigured  to  “srl_dlink,  ”  and  the  password  remained  blank  for  ease  of  access. 

Through  the  Due  code,  which  is  written  in  the  C  programming  language,  the  Wi¬ 
Fi  shields  on  the  links  are  instructed  to  join  the  SRL  network  (Figure  52).  The  router 
employs  a  Dynamic  Host  Configuration  Protocol  (DHCP).  That  is,  when  devices  join  the 
network,  the  router  assigns  each  device  an  internet  protocol  (IP)  address,  which,  in  the 
default  configuration,  changes  each  time  the  device  joins  the  network.  To  avoid  manually 
changing  the  device  IPs  each  time  in  both  the  Simulink  model  and  the  Due  code,  the 
router  was  configured  to  assign  the  same  IPs  to  the  same  hardware,  represented  by  a 
unique  media  access  control  (MAC)  address,  each  time.  Similarly,  port  numbers  were 
hard-coded  into  the  Due  code  to  standardize  the  connection  process.  Port  numbers  are 
not  necessary  for  the  connection  itself,  but  they  are  necessary  to  designate  the  destination 
of  the  datagram  packet.  For  example,  Link  1  connects  to  the  FSS/Control  Laptop 
and  sends  data/listens  to  port  #25010;  the  MAC  address  of  its  Wi-Fi  shield  is 
0x78, 0xc4,0x0e, 0x01, 0xc5,0xbl.  The  excerpt  of  the  Arduino  code  below  defines  the 
network,  the  FSS  IP  and  its  listening  port,  the  FSS  ports,  and  the  MAC  addresses  in 
reverse. 
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//Wireless  Parameters 

int  stat_w  =  WL_IDLE_STATUS; 

♦define  ssid  ”dlink_srl"  //  Network  SSID  (name) 

♦define  FSS_IP  ”192.168.0.100"  //  IP  of  the  FSS 
♦define  localPort  4097  //  Local  port  to  listen  on 

//FSS  ports  (will  be  assigned  depending  on  mac  address) 

♦define  FSS_PORT_A  25010 
♦define  FSS_PORT_B  25020 
♦define  FSS_PORT_C  25030 
♦define  FSS_PORT_D  25040 

byte  mac[6];  //Holds  Wi-Fi  shied  mac  address 
int  FSS_port=  FSS_PORT_A;  //Holds  FSS  port 

//Mac  addresses  of  Wifi  shields  (in  reverse!) 

♦define  MAC_A  Oxbl, 0xc5, 0x01, OxOe, 0xc4, 0x78 
♦define  MAC_B  0x7a, Oxfb, 0x01, OxOe, 0xc4, 0x78 
♦define  MAC_C  0x49, 0xc5, 0x01, OxOe, 0xc4, 0x78 
♦define  MAC_D  0x47, 0xc5, 0x01, OxOe, 0xc4, 0x78 

Figure  52.  Code  Defining  Network  Parameters,  Port  Numbers, 

and  MAC  Addresses. 


A  second  aspect  of  the  data  transfer  process  that  bears  discussion  is  the  message 
protocol  itself.  The  system  employs  the  User  Datagram  Protocol  (UDP).  Unlike  its  more 
reliable  cousin,  the  Transmission  Control  Protocol  (TCP),  UDP  sends  datagrams  blindly 
without  knowing  for  certain  that  there  is  a  device  on  the  other  end  to  receive  it  [43].  This 
setup  allows  for  rapid  messaging  and  simplifies  the  requirement  to  translate  the  data 
packets,  but  message  information  can  be  lost  during  transmission  or  arrive  in  a  different 
order  [43].  For  this  investigation,  simplified  data  translation  was  a  highly  desirable 
characteristic,  but  as  will  be  discussed  in  a  later  portion  of  this  chapter,  the  UDP  protocol 
led  to  data  management  challenges. 

2.  The  Driver  Interface  and  the  Arduino  Due  Code 

The  Due’s  software  script  initializes  contact  with  the  link’s  motor  via  on-board 
code.  As  a  serial  interface,  the  RS232  cable  connects  the  two  pieces  of  hardware  and 
enables  communication  by  text.  The  initialization  code  sets  the  maximum  acceleration 
and  deceleration  rates  (5000  counts/scc”,  an  arbitrarily  high  number),  sets  the  initial 
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velocity  (zero  counts/sec),  enables  the  amplifier,  and  requests  position  and  velocity  data 
from  the  driver.  Upon  receipt  of  the  velocity  command  from  the  Simulink  model,  the 
Arduino  stack  passes  the  command  to  the  driver.  The  American  Standard  Code  for 
information  Interchange  (ASCII)  strings  used  to  achieve  this  functionality  are  shown  in 
Table  1.  For  a  comprehensive  guide  to  the  driver’s  ASCII  interface  and  a  list  of  available 
commands,  see  [25]. 

Table  3.  ASCII  Strings  for  Due-to-Driver  Communication.  Source:  [25]. 


Function 

ASCII  String 

Set  Acceleration 

s  r0x36 

Set  Deceleration 

s  r0x37 

Set  Initial  Velocity 

s  r0x2f 

Enable  Amplifier 

s  r0x24 

Request  Position  Data 

g  r0x32 

Request  Velocity  Data 

g  r0xl8 

Request  Current  Data 

g  rOxOc 

The  driver  communicates  the  command  to  the  motor  and  requests  position  and 
velocity  data.  The  motor  sends  position  and  velocity  data  back  via  the  encoder  cable. 
Meanwhile,  the  torque  sensor  is  read  back  through  the  load  cell  amplifier.  All  of  the  data 
(position,  velocity,  and  torque)  is  read  synchronously  by  the  Due’s  script  and  routed  back 
through  the  Wi-Fi  shield,  through  the  router,  and  to  the  UDP  receive  block  in  Simulink, 
which  allows  for  analysis.  Unit  conversions  take  place  within  the  Due  script,  an 
improvement  to  the  previous  system  in  which  raw  units  from  the  driver  (e.g.,  “counts”) 
were  converted  to  more  meaningful  measurements  (e.g.,  radians)  via  a  separate 
Simulink  model  before  analysis.  For  the  complete  Arduino  code,  see  Appendix  C: 
OnBoardLink.ino. 
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3. 


The  Simulink  Model 


a.  The  Send/Receive  Switch 

The  Simulink  block  diagram  in  Figure  53  shows  the  simple  on/off  switch  used  to 
command  a  single  link.  For  the  kinematic  and  dynamic  calibration,  constant  positive  or 
negative  velocities  were  sent  via  Wi-Fi  to  the  particular  link.  The  predetennined  port 
numbers  and  IP  addresses  were  used.  On  the  receive  side,  position,  velocity,  torque  data, 
and  a  fourth  quantity,  the  checksum  term  (discussed  subsequently),  return  over  the 
network.  In  the  composite  ground  station  file,  four  such  switches  were  used,  one  to 
command  each  of  the  four  links. 


|  0.1  I - ►-<. 

r°~i — 


— ►  single  — 

Data  Type  Conversion3 


► 


Data 


192.168.0.101 
Port:  4097 


Data  Send 
Link  1 


Telemetry 


Figure  53.  Send/Receive  Switch  for  Link  Control. 
b.  The  Data  Processing  Subsystem 

A  breakout  of  the  Data  Processing  Subsystem  is  given  in  Figure  54  and  itself 
consists  of  two  subsystems,  the  Checksum  Subsystem  and  the  Enabled  Subsystem.  The 
internals  of  the  Checksum  Subsystem  and  the  Enabled  Subsystem  are  shown  in  Figures 
55  and  56,  respectively.  The  Data  Processing  Subsystem  exists  to  receive  the  data  from 
the  links,  verify  the  integrity  of  the  data  packets,  and  transform  those  data  packets  into  a 
useful  form  for  analysis. 
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Before  transmission  from  the  link,  each  data  packet  initially  consists  of  position, 
velocity,  and  torque  data.  Because  the  data  stream  may  be  corrupted  during  transmission, 
however,  the  microprocessor  code  creates  a  pseudo-unique  checksum  tenn  based  on  the 
numerical  values  of  the  position,  velocity,  and  torque.  These  four  terms,  then,  are 
transmitted  inside  of  a  single  data  packet,  which  the  ground  station  receives.  This  data 
packet  passes  through  the  Checksum  Subsystem  (Figure  55).  The  purpose  of  the 
Checksum  Subsystem  is  to  verify  that  a  complete  and  uncorrupted  data  packet  has  arrived 
from  the  Link.  To  accomplish  this,  it  reverses  the  process  used  to  create  the  checksum 
tenn.  The  first  three  pieces  of  information  (position,  velocity,  and  torque)  are  stripped 
away  from  the  raw  data  stream,  multiplied  by  one  hundred,  and  added  together.  This  sum 
is  subtracted  from  the  original  checksum  term  and  added  to  one  hundred.  If  all  bits  arrive 
unconupted,  the  final  result  of  the  additions  and  subtraction  should  be  zero  (to  within  a 
small  tolerance). 

The  output  of  the  Checksum  Subsystem  controls  the  behavior  of  the  Enabled 
Subsystem.  If  the  result  of  the  checksum  calculation  is  non-zero,  the  Enabled  Subsystem 
is  disabled,  preventing  the  corrupt  data  packet  from  passing  through.  If  the  result  of  the 
checksum  calculation  is  zero,  the  Enabled  Subsystem  is  enabled,  allowing  the  data  packet 
to  pass  through.  Since  it  is  highly  unlikely  that  a  corrupted  data  packet  will  result  in  a 
zero  calculation,  the  checksum  ensures  that  only  valid  data  packets  are  included  for 
analysis. 


Subsystem 


Torque  [Nm] 


Figure  54.  Breakout  of  the  Data  Processing  Subsystem. 
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Figure  55.  Breakout  of  the  Checksum  Subsystem. 
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Figure  56.  Breakout  of  the  Enabled  Subsystem. 

The  need  for  the  checksum  became  apparent  during  initial  testing  of  the 
communications  architecture  when  data  packets  were  dropping  or  being  received 
incorrectly.  The  limitations  of  UDP  were  manifesting  themselves.  During  the  data 
transfer  testing  phase,  it  was  observed  that  the  data  sent  from  the  Arduino  (position, 
velocity,  and  torque)  would  be  received  into  Simulink  in  that  order.  At  random  intervals, 
however,  the  order  of  the  data  received  would  swap,  usually  by  one  place.  For  example, 
the  position-velocity-torque  data  stream  might  switch  to  a  velocity-torque-position  data 
stream.  Further,  data  swaps  were  observed  that  involved  two  variables  switching  places,  a 
seemingly  random  behavior  that  made  plotting  and  analysis  of  the  data  extremely 
inconvenient  if  not  outright  impossible.  An  extensive  debugging  of  the  Arduino’s  C  code 
resulted  in  improved  performance  mainly  through  the  manipulation  of  driver  read  delays 
and  an  increased  buffer  size,  but  the  interim  solution  was  enabling  the  blocking 
functionality  in  the  Simulink  UDP  receive  block. 

The  blocking  functionality  enables  retention  of  data  until  a  complete  packet  of 
new  data  is  received.  The  maximum  amount  of  time  that  Simulink  will  wait  between 
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packets  is  a  configurable  parameter  that  was  arbitrarily  set  at  ten  seconds.  Thus,  if 
Simulink  did  not  receive  a  new  complete  data  packet  from  the  Arduino  subsystem  within 
ten  seconds,  the  simulation  would  time-out.  Enabling  the  blocking  mitigated  the  data 
swapping  problem  but  introduced  the  new  problem  of  the  simulation  timing  out.  In 
several  cases,  data  collects  of  significant  length  (on  the  order  of  minutes)  could  be 
achieved  before  the  simulation  timed  out.  The  interim  solution  was  finally  replaced  by  a 
better-behaved  solution,  the  implementation  of  the  Checksum  Subsystem  that  detects 
error  packets  and  only  enables  complete  packets  to  be  passed  through  for  analysis.  With 
the  implementation  of  the  checksum,  the  data  swapping  and  system  time-outs 
disappeared,  and  data  could  be  collected  with  confidence. 

c.  The  Capture  Function 

After  any  residual  motion  damped  out  of  the  base-manipulator  system,  the 
positions  of  the  joint  motors  (qt)  as  measured  by  the  encoders  were  captured  via 

Simulink’s  capture  function,  which  was  also  included  in  ground  station  workspace 
(Figure  57).  Depressing  the  capture  button  in  Simulink  enables  the  trigger,  momentarily 
turning  the  “0”  at  the  top  of  the  diagram  into  a  “1”  to  inform  the  operator  that  the  capture 
has  been  successful.  The  vector  of  joint  positions  was  passed  into  MATLAB;  multiple 
measurements  built  the  initial  vector  into  a  matrix  containing  all  measurements  to  be  used 
for  the  kinematic  calibration.  The  Simulink  capture  happened  at  the  same  time  as  the 
capture  of  the  end  effector  and  base-spacecraft  state  vectors  via  VICON. 


Figure  57.  Capture  Function 
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4. 


The  VICON  Interface 


As  it  is  used  in  the  NPS  SRL,  the  VICON  system  consists  of  ten  infrared  cameras, 
an  independent  server,  and  the  proprietary  software  to  operate  the  system.  The  motion 
capture  cameras  are  depicted  in  the  experimental  setup  shown  in  Figure  58.  The  array  of 
cameras  detect  the  light  as  it  bounces  off  of  a  series  of  silver  spherical  markers,  and  this 
telemetry  is  sent  to  the  main  VICON  computer  via  Ethernet  cable  where  the  inputs  of  the 
multiple  cameras  are  correlated. 


Floating  Spacecraft 
Simulator 


Motion  Capture 
Cameras 


Floating  Spacecraft 
Simulator 


Modular 


Granite  Monolith 


Figure  58.  Experimental  Setup  Including  VICON  Cameras.  Source:  [44]. 
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Figure  59.  Silver  Spheres  Serve  as  Markers  for  VICON.  Source:  [44]. 

A  screenshot  of  the  VICON  tracking  software  is  shown  in  Figure  60  where  the 
silver  reflective  spheres  appear  as  gold  balls.  A  reference  frame  is  attached  to  the  left¬ 
most  ball,  which  is  attached  to  the  end  of  Link  4.  This  ball  represents  the  location  of  the 
notional  end  effector.  As  with  the  telemetry  capture  from  the  links,  the  telemetry  capture 
from  VICON  is  accomplished  manually  via  a  capture  button  and  collected  via  a 
MATLAB  script.  The  interface  between  the  VICON  system  and  MATLAB  requires 
VICON’s  proprietary  Datastream  SDK  software  to  be  in  the  MATLAB  script’s  path.  This 
software  is  available  at  [45].  The  MATLAB  script  that  captures  the  data  for  the  kinematic 
calibration,  including  the  state  vectors  of  the  base  spacecraft  and  the  end  effector,  is 
included  in  Appendix  E:  ViconCalibrate.m. 
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Figure  60.  Screenshot  of  the  VICON  Tracking  Software. 


5.  Potential  for  an  Ad  Hoc  Network 

Although  this  configuration  proved  adequate,  its  limitations  were  also  apparent. 
First,  the  Arduino  Due  is  not  capable  of  creating  an  ad  hoc  Wi-Fi  network.  If  it  were,  the 
microprocessor/Wi-Fi  shield  combination  could  serve  as  a  router,  and  an  external  router 
would  not  be  necessary.  Such  a  configuration  is  desirable  because  it  simplifies  the 
hardware  used  in  the  data  transmission  architecture  and  creates  a  more  realistic  scenario; 
the  FSS  communicates  with  each  link  via  a  self-contained  network.  A  more  capable 
processor,  such  as  a  Raspberry  Pi,  would  be  capable  of  creating  such  an  ad  hoc  Wi-Fi 
network  and  has  the  advantage  of  further  simplifying  the  link  hardware;  a  Raspberry  Pi 
with  Wi-Fi  dongle  can  replace  the  whole  Arduino  Due,  Arduino  Wi-Fi  shield,  and  the 
RS232  shield. 
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V.  KINEMATICS  AND  DYNAMICS  CALIBRATION 


A.  GENERAL  OVERVIEW 

To  understand  how  the  spacecraft/manipulator  system  will  behave,  its  mechanics 
must  be  understood.  The  mechanics  of  a  system  consists  of  its  kinematics  and  its 
dynamics.  Broadly,  kinematics  is  the  study  of  the  motion  of  an  object  without 
consideration  of  its  causes.  Dynamics  is  the  study  of  the  motion  with  consideration  of  its 
causes  (i.e.,  forces  and  torques).  This  chapter  addresses  the  kinematic  characterization  of 
the  four-link  manipulator/FSS  system  followed  by  the  dynamic  characterization.  The 
mathematical  underpinnings  of  the  model  are  discussed  in  the  context  of  their 
application;  that  is,  the  discussion  follows  the  flow  of  the  MATLAB  code,  the  SPART 
model  [24],  The  portions  of  the  SPART  model  relevant  to  this  thesis  are  contained  in 
(Appendices  E-P).  While  the  SPART  model  is  deigned  to  work  in  for  up  to  six  degrees 
of  freedom,  the  discussion  of  the  application  of  the  model  in  this  section  only  concerns 
itself  with  the  3DOF  observable  in  experimentation. 

A  second  function  of  the  SPART  model  is  to  provide  the  calibration  functionality. 
The  calibration  is  done  to  refine  the  system  parameters  that  are  difficult  to  measure,  a 
process  that  the  subsequent  chapter  discusses  in  detail.  The  calibration  scripts,  one  for  the 
kinematic  calibration  and  one  for  the  dynamic  calibration,  allow  for  comparison  of  the 
initially  estimated  system  parameters  and  the  real-world  observations.  With  accurate 
knowledge  of  the  system  parameters,  the  system  can  be  more  precisely  controlled. 

B.  KINEMATICS  OVERVIEW 

1.  Definition  of  Terms 

To  characterize  the  kinematics  of  a  unitary  object,  it  is  necessary  to  identify 
the  position,  orientation,  linear  velocity,  and  angular  velocity  of  the  object.  The 
MARSMAN  system,  however,  is  not  a  unitary  object,  which  significantly  complicates 
the  characterization  of  its  kinematics.  For  the  FSS  with  n  link  attachments,  the 
characterization  of  the  kinematics  requires  definition  of  the  kinematic  properties  not  only 

of  the  base,  but  also  of  each  link  and  joint  and  ultimately  the  end  effector.  These 
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parameters  are  highly  coupled  and  change  at  each  time  step  in  the  mathematical 
propagation. 

The  SPART  model  partly  exists  to  model  the  system  behavior  (e.g.,  the  positions 
of  the  links,  joints,  and  end  effector  at  a  given  time).  It  necessarily  calculates  the 
kinematic  parameters  first.  This  main  script  for  kinematics  is  given  in  Appendix  F: 
KinematicsSerial.m.  This  script  calls  on  two  supporting  functions.  The  script  in 
Appendix  G:  DH  Rs.m  calculate  the  rotation  matrix  R  and  the  translation  vector  s  based 
on  the  Denavit-Hartenberg  (DH)  parameters  and  joint  variables  (qm).  The  script  in 

Appendix  H:  Skew  Sym.m  converts  the  designated  vector  into  the  fonn  of  a  skew 
symmetric  matrix. 

2.  The  Base 

The  base  itself  can  be  treated  as  a  rigid,  unitary  object  with  3DOF  (translation  in 
the  x  and  y  directions  and  rotation  about  the  z  axis).  Its  position  relative  to  the  reference 
frame  is  described  in  terms  of  a  state  vector  ( q0 ),  a  6x1  column  matrix  that  contains 
information  about  the  base’s  position  in  terms  of  linear  motion  ( x,y,z  components)  and 
rotational  motion  (0x,0  ,0z);  see  Equation  4.  Collectively,  the  angles  0x , 0 , 0  arc 
referred  to  as  the  Euler  angles. 


x 


y 


A. 


(4) 


Since  the  experiments  occur  on  a  flat  table,  the  z  component  of  the  linear  motion 
will  always  be  zero:  the  MARSMAN  cannot  move  up  and  down.  Similarly,  it  can  only 
rotate  about  the  z  axis,  so  the  0X  and  0  components  can  be  assumed  to  be  zero.  The 

velocity  vector  ( q0 )  and  the  acceleration  vector  ( q0  )  follow  the  same  form  as  q0  and  are 
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simply  the  first  and  second  derivatives  of  q0 .  As  mentioned,  the  number  of  degrees  of 

freedom  of  the  FSS  is  limited  by  the  current  experimental  environment,  but  the 
simulation  model  could  easily  accommodate  6DOF  data  if  the  opportunity  for  more-than- 
3DOF  experiments  arises. 

3.  The  Joints 

For  characterizing  the  kinematics  of  an  n-link  manipulator  arm,  one  must  know 
the  relative  positions  and  velocities  of  each  joint  on  the  ann  with  respect  to  the  base.  The 
joint  position  vectors  ( qm  )  and  the  joint  velocity  vector  ( qm )  are  nx  1  column  vectors.  For 

this  experimental  campaign,  the  VICON  system  measured  the  end  effector  and  base 
positions  and  passed  them  to  the  MATLAB/Simulink  environment  via  Wi-Fi.  The 
servomotor  driver  provided  telemetry  (angular  position  and  velocity)  for  each  link. 

4.  Rotation  and  Joint  Transformation  Matrices 

The  Euler  angles  of  the  spacecraft  base  are  used  to  create  the  3x3  rotation  matrix 
(Rh)  that  expresses  the  orientation  of  the  base  with  respect  to  the  inertial  frame.  This 
model  arbitrarily  employs  the  common  1-2-3  rotation  sequence  shown  in  Equation  5 
[46].  Other  rotation  sequences  could  just  as  readily  be  employed. 


r u  = 


cos(0,)cos(ft) 

-cos(ft.)sin(ft) 

sin(6»v) 


cos  (8X )  sin(ft )  +  cos(ft )  sin(#t )  sin(#v ) 
cos(6t )  cos(ft )  -  sintfit )  sin(  9y )  sin( 0, ) 
-cost^JsinC^J 


sin(  0X )  sin(  9,)-  cos(0x )  cos(  9, )  si  ti(6*v ) 
cos(  ft )  sin(ft  )  +  cos(0t )  sin(0v )  sin(ft ) 
cos(ft)cos(ft() 


(5) 


The  rotation  matrix  only  accounts  for  the  rotational  motion  of  the  base.  To 
account  for  both  the  rotational  and  translational  motion,  a  transfonnation  matrix  is 
necessary.  The  transformation  matrix  of  the  base  is  of  the  general  form  shown  in 
Equation  6  and  includes  the  rotation  matrix  and  a  3x1  translation  vector  r  =  \x;  y;  z] 
describing  the  base’s  position  with  respect  to  the  inertial  frame. 
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Like  the  base,  each  of  the  joints  also  requires  a  transformation  matrix  to  describe 
the  motion  of  the  respective  joint.  Whereas  the  transformation  matrix  of  the  base  relates 
its  position  to  the  inertial  frame,  the  transfonnation  matrix  of  each  joint  relates  its 
position  to  the  position  of  the  previous  joint.  The  transfonnation  matrices  of  Joints  2 
through  n+1  (where  n+ 1  is  the  end  effector)  follow  the  same  pattern.  Because  Joint  1  is 
attached  to  the  base,  its  motion  is  described  based  upon  its  relationship  to  the  base. 
Equation  7  shows  Joint  l’s  transformation  matrix,  which  remains  constant  throughout 
maneuvers.  Joint  1  is  an  estimated  18.3cm  from  the  center  of  mass  (CoM)  of  the  base  in 
the  x  direction  (the  calibration  process  will  help  refine  this  estimate).  The  3x3  identity 
matrix  ( 13 )  occupying  the  top  left  of  Equation  7  is  indicative  of  Joint  1  sharing  the  same 
orientation  as  the  base. 


T  = 

1j\ 


18. 33  cm 


13 

L 

0  0 


0 

J  0 

0  1 


(7) 


To  obtain  the  transfonnation  matrix  of  Joint  1  in  the  inertial  frame  (/),  the 
matrices  from  Equations  6  and  7  are  multiplied  together  (Equation  8).  Calculation  of  the 
transformation  matrices  of  subsequent  links  requires  use  of  DH  parameters. 


%,  =  %%  1 


(8) 
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5.  Denavit-Hartenberg  (DH)  Parameters 

Calculating  the  transformation  matrices  for  the  remaining  joints  (Joints  2  through 
n+1)  requires  recursive  application  of  the  DH  parameters.  The  DH  parameters  are  a 
convention  used  to  conveniently  define  successive  reference  frames,  each  assigned  to  a 
joint  of  the  manipulator  [47].  By  this  method,  any  joint  reference  frame  can  be  ultimately 
described  in  terms  of  the  inertial  reference  frame.  The  symbolic  representation  of  the 
parameters  varies  within  literature;  this  thesis  employs  the  symbols  d,  q,  a,  a .  Table  2 
lists  the  parameters  and  a  description  of  their  function,  while  Figure  61  depicts  the 
parameters  visually. 


Table  4.  Denavit-Hartenberg  Parameters.  Adapted  from  [44], 


Parameter 

Definition 

^i,M 

Translational  distance  between  the  Jl  and  Jl  ,  frames  along  the  kt  ax i s 

(6,i+i 

Angle  of  rotation  from  (  to  iM  along  k, 

ai,M 

Distance  along  the  common  normal  between  k  and  kM 

«M+ 1 

Angle  of  rotation  from  ki  to  ki+l  along  iM 

Figure  61.  Visual  Depiction  of  the  Denavit-Hartenberg  Parameters.  Source:  [44], 

The  generic  form  of  the  DH  transformation  matrix  is  given  in  Equation  9  [47]. 
Each  joint  requires  its  own  DH  transformation  matrix.  Since  each  joint  is  rigidly  affixed 
to  the  previous  link  its  parameters  are  dependent  upon  that  link’s  position  at  any  step  in 
the  numerical  simulation. 
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DH  = 


cos  (qn_x)  -sin(^ll_1)cosan_1 
sin(^,_,)  cos(<7„_1)cosa„_1 

0  sin  an_x 

0  0 


sin^/„  ,)sin  an  cn  ,  cos(tf„_,) 
-  cos(^_! )  sin  c„_j  sin(<7;;  j ) 

cos«„  ,  dn_ i 

0  1 


(9) 


In  the  case  of  a  3DOF,  planar  manipulator,  the  generic  form  shown  above 
simplifies  greatly.  First,  the  MARSMAN  is  unable  to  translate  along  the  z  axis,  which 
makes  d  —  0  throughout  the  simulation.  Second,  the  z  axes  of  all  joints  are  parallel 
(pointing  upward),  so  rotation  about  the  x  axis  is  never  necessary  when  aligning  reference 
frames.  Thus,  a  —  0  throughout  the  simulation,  and  the  DH  matrix  simplifies  to  the  form 
shown  in  Equation  10. 


cos  (qn_x) 


DH  = 


0 

0 


1  0  cn_xcos{qn_x) 

1  0  cn_x  sin(g;i  j) 

0  1  0 
0  0  1 


(10) 


With  the  DH  matrix  established,  calculating  the  transformation  matrices  of  Joints 
2-4  in  the  inertial  reference  frame  is  only  a  matter  of  matrix  multiplication.  However, 
because  the  DH  parameters  are  updated  whenever  a  joint  moves,  this  multiplication 
operation  must  take  place  numerous  times.  Equation  1 1  provides  the  equations  for  the 
Joint  2-4  and  the  end  effector  transformation  matrices.  In  the  kinematic  calibration 
process,  the  transformation  matrix  of  the  end  effector  TEE  is  the  only  portion  that  requires 
further  manipulation. 


^72  —  ^71^^71 

^73  =  =  Tj\DH  jxDH  j2 

TJA=TnDHn=TnDHnDHJ2DHn 

Tee  =TJ4DHJ4  =TnDHnDHJ2DHJ3DHJA 
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6. 


Link  Transformation  Matrices 


Transformation  matrices  for  the  manipulator  links  are  calculated  much  the  same 
as  the  transformation  matrices  of  the  joints.  The  transformation  matrix  of  Link  1  ( Ji+'  Tu  ) 

resides  in  the  reference  frame  of  the  /+/  joint  [23].  The  vector  b  is  the  vector  from  the 
center  of  mass  of  Link  i  to  the  /+/  joint  and  accounts  for  the  translational  motion  of  the 
link  just  as  the  vector  r  accounted  for  the  translational  motion  of  the  spacecraft  base  in 
the  base  transformation  matrix.  Since  the  ith  link  rotates  with  the  (i+l)th  joint,  the  portion 
of  the  transformation  matrix  that  describes  the  link’s  rotation  is  simply  a  3x3  identity 
matrix  (Equation  12).  To  obtain  the  transformation  matrix  of  a  link  in  the  inertial  frame 
(,Tli)  ,Ji*'TLi  from  Equation  12  is  multiplied  by  'T]+l  (Equation  13). 


1 T 

1  Li 


1  ~b[ 


L 


L  J 

0  0  0 


-K 

-bL 


(12) 


’T 

1  Li 


(13) 


C.  DYNAMICS  OVERVIEW 

At  this  point  in  the  mathematical  model,  all  of  the  parameters  necessary  for  the 
kinematic  calibration  are  computed,  and  a  discussion  of  the  property  known  as  “twist”  is 
necessary.  Broadly,  the  twist  vector  of  a  link  ( li )  compacts  the  angular  and  linear 
velocities  as  shown  in  Equation  14  [48]. 


(14) 


The  model,  KinematicSerial.m,  continues  by  calculating  the  twist  propagation 
matrices  (BijandBi0) ,  the  velocity  transformation  matrix  (P0)  ,  and  the  twist  propagation 
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vectors  (jo,) .  These  parameters  are  important  for  the  characterization  of  the  system 
dynamics.  The  twist  vector,  then,  follows  the  relationship  given  in  Equation  15  [48]. 

t,  =  Bijtj  +  Piq,  (15) 

1.  Twist  Propagation  Matrices  ( Btj  and  Bj0 ) 

The  6x6  twist  propagation  matrix  Btj  accounts  for  angular  and  linear  velocities  of 
pairs  of  joints  by  taking  the  positional  difference  (r  -  r)  of  successive  joints  and  turning 
that  vector  into  a  3x3  skew  symmetric  matrix  rj  [48].  Similarly,  the  twist  propagation 
matrix  Bi0  accounts  for  the  angular  and  linear  velocities  between  the  ith  link  and  the  base 
(r0  —  r )  or  r0* .  Interestingly,  the  skew  symmetric  functionality  is  not  built  into  MATLAB 
as  an  organic  function,  so  the  in  Appendix  H:  SkewSym.m  was  again  employed  to 
convert  the  r.  and  r0i  vectors  into  the  necessary  form.  The  fonn  of  Btj  is  shown  in 

Equation  16  where  13  denotes  a  3x3  identity  matrix  and  03  denotes  a  3x3  zero  matrix  [48]. 
The  form  of  Bi0  is  the  same  with  the  exception  that  r0*  replaces  rj  . 


2. 


(16) 


Velocity  Transformation  Matrix  ( P0 )  and  Twist  Propagation  Vector 

(Pj 


The  velocity  transformation  matrix  accounts  for  the  twisting  effects  of  the 
spacecraft  base  on  the  system  dynamics.  It  is  shown  in  Equation  17,  “a  6x6  matrix  that 
contains  the  base-spacecraft  rotation  matrix”  in  the  inertial  frame  [48]. 


(17) 
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The  twist  propagation  vectors  account  for  the  twisting  effects  of  each  joint  on  the 
system  dynamics.  An  explanation  of  the  twist  propagation  vector  requires  introduction  of 
the  r ,  /, , el , and  g(.  vectors.  Figure  62  depicts  these  vectors  pictorially. 


Inertial  Frame 

V 

Figure  62.  Depiction  of  Dynamic  Model  Vectors:  Source:  [48]. 

Fortunately,  these  vectors  do  not  require  new  calculations;  they  are  made  by 
parsing  pieces  from  already  existing  transformation  matrices.  For  example,  r.  is  the  first 
three  terms  of  fourth  column  of  the  ith  link  transformation  matrix  (as  seen  previously)  and 
accounts  for  the  link’s  translational  motion  in  inertial  space;  in  other  words,  r.  is  the 
vector  from  the  origin  of  the  inertial  frame  to  the  center  of  mass  of  the  ith  link.  The  vector 
/( is  similar  to  r  but  comes  from  the  joint  transformation  matrix  (instead  of  the  link 

transformation  matrix)  in  the  inertial  frame.  It  accounts  for  the  joint’s  translational 
motion  in  inertial  space  and  represents  the  vector  from  the  origin  of  the  inertial  frame  to 
the  origin  of  the  ith  joint  reference  frame.  The  vector  e  gives  the  orientation  of  the  joint’s 
rotation  axis  (the  first  three  rows  of  the  third  column  of  each  joint  transformation  matrix). 
In  the  planar  case,  e  is  simply  a  unit  vector  indicating  rotation  about  the  z  axis.  Finally, 
the  geometric  vector  g(is  the  difference  between  rand  /, .  Since  the  joints  in  this 

experiment  are  revolute,  the  magnitude  of  g(  does  not  change.  With  these  four  vectors 
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defined,  the  manipulator  twist  propagation  vector  (pm),  is  calculated  as  shown  in 
Equation  18  [48]. 


D.  KINEMATIC  CALIBRATION 


(18) 


The  explanation  thus  far  has  dealt  with  the  mathematical  manner  in  which  the 
nominal  kinematics  of  the  system  are  calculated  largely  via  KinematicSerial.m  and  its 
supporting  functions.  The  kinematic  calibration  process  is  primarily  concerned  with  using 
the  model  and  the  experimentally  measured  position  of  the  end  effector  to  refine  the 
system  parameters.  To  that  end,  the  end  effector  was  measured  in  a  series  of  poses. 


With  the  four-link  manipulator  attached  to  the  base  spacecraft  and  floating  on  the 
monolith,  the  manipulator  links  were  given  a  number  of  random  commands  ( m ). 
Following  each  command,  the  state  of  the  end  effector  ( xee )  was  measured  using  the 
VICON  system,  and  the  measurements  of  all  joint  angles  (as  read  by  the  driver  and 
passed  through  UDP  to  Simulink)  were  recorded  in  an  mxn  matrix  (q).  When  combined 
with  the  VICON  measurements  of  the  base  spacecraft  position  ( qO ),  the  homogeneous 
transfonnation  matrix  describing  the  end  effector  position  (Tee)  can  be  solved  for 
numerically.  From  this,  the  x  and  y  translational  and  the  0.  rotational  components  of  the 
end  effector  in  the  inertial  frame  are  extracted. 


Based  on  the  kinematic  model’s  estimates  of  the  end  effector  state,  small 
variations  were  induced  into  the  x,  y,  and  0_  measurements  to  see  how  much  a  small 
change  in  each  parameter  would  affect  the  output  of  the  kinematic  model  ( k ).  These 
partial  derivatives  were  collected  into  the  kinematic  calibration  matrix  ( O  ),  which  is  of 
the  general  form  shown  in  Equation  19.  Again,  the  planar  case  simplifies  the  calculations. 
The  parameters  d  and  a  do  not  change  over  time,  so  they  are  inconsequential  to  the 
calculations.  The  q  parameter  for  all  four  joints  and  the  a  parameters  for  all  four  links, 
however,  must  be  incrementally  adjusted  to  properly  tune  the  joint  measurements. 
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Further,  refinement  of  the  location  of  Joint  1  with  respect  to  the  base’s  center  of  mass  is 
necessary,  so  the  parameters  xB  JX ,  yB  n ,  0B  JX  must  be  similarly  manipulated. 

®  =^Sk/Sa  Sk/Sd  Sk/SxBJX  Sk/SyBJl  5k/89BJ^  (19) 

With  each  iteration,  the  nominal  end  effector  state  vector  ( xn)  was  compared  to 
the  experimentally  measured  end  effector  state  vector  ( xm  )  and  the  differences  compiled 
into  the  matrix  Ax  (Equation  20)  [47]. 


(20) 

The  matrix  is  a  combination  of  the  of  the  kinematic  calibration  matrix  (®) 
and  the  nominal/measurcd  differences  ( Ax  )  as  shown  in  Equation  20.  An  iterative  batch 
least-squares  method  was  used  to  drive  down  the  values  of  A  A ,  the  “parameter 
variations  with  respect  to  the  nominal  values”  [47].  When  the  least-squares  solution 
returned  a  A  A  vector  within  a  pre-determined  tolerance  (Equation  21),  the  solution 
provided  insight  into  how  much  the  estimated  parameters,  varied  from  the  true  system 
behavior. 


Af=(®r®)H®rAr  (2[) 

E.  DYNAMIC  CALIBRATION 
1.  Overview 

While  the  kinematic  calibration  concerns  itself  with  refinement  of  kinematic 
parameters,  the  dynamic  calibration  concerns  itself  with  refinement  of  the  dynamic 
parameters.  The  two  calibration  processes  work  hand-in-hand;  the  offsets  determined  by 
the  kinematic  calibration  feed  the  dynamic  calibration  model  as  an  initial  guess.  Thus,  it 
is  not  possible  to  complete  the  dynamics  calibration  without  first  completing  the 
kinematics  calibration. 

Functionally,  both  processes  share  similarities.  The  same  Simulink  controller, 
VICON  system  setup,  and  telemetry  data  capturing  technique  was  used  for  the  dynamic 
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calibration.  All  measurements  were  taken  with  the  spacecraft/manipulator  system  in  a 
stationary  position;  measurements  were  not  taken  while  the  motors  were  active  or  while 
the  residual  vibration  from  the  maneuver  was  naturally  damping  out.  Unlike  the 
kinematics  calibration,  the  dynamics  calibration  required  capturing  the  three  sets  of 
position  data  both  prior  to  a  maneuver  and  following  the  maneuver:  the  initial  ( qm  )  and 
final  (q02)  positions  of  the  base,  the  initial  (qn)  and  final  positions  ( qi2 ),  and  the  initial 
( xeeX )  and  final  ( xee2 )  positions  of  the  end  effector.  The  main  script  for  the  dynamics 

calibration  is  in  Appendix  K:  MARSMAN_DynCal,  but  before  proceeding  with  the 
actual  calibration,  several  supporting  functions  are  employed  to  account  for  the  system 
dynamic  effects  and  produce — as  an  interim  objective — the  nominal  state  vector  of  the 
base-manipulator  system  (q0x,q0v,q0g)  ■ 

To  obtain  the  nominal  state  vector,  the  link  and  body  rotation  matrices  (f?Land 
R0,  respectively;  both  are  outputs  of  Kinematics  Serial.m)  are  first  used  to  construct  the 
inertia  matrices  of  the  base  (70)  and  manipulator  ( Im ) .  The  inertia  matrices  and  the  twist 
propagation  matrices  ( Btj  and  Bj0 ,  also  products  of  Kinematics  Serial.m)  contribute  to 
the  mass  composite  body  matrices  of  the  base  (M0)  and  the  manipulator  (M m) .  With 
these,  the  generalized  inertia  matrices  of  the  base  ( H0 )  and  manipulator  (Hm )  can  be 
calculated  and  used  to  determine  the  derivative  of  the  base  position  ( q0 ) .  The  vector  is 
integrated  and  variation  applied  to  the  components  (q0x,q0y,q0g)  for  construction  of  the 
dynamic  calibration  matrix  (cp) . 

2.  Inertia  Matrices 

The  link  and  body  rotation  matrices  (f?Land  R0)  from  Kinematics  Serial.m  are 
passed  to  a  supporting  function  (see  Appendix  L:  I  I. m).  The  body  rotation  matrix  (R0) 
is  multiplied  by  the  previously  defined  inertia  of  the  spacecraft  to  produce  the  inertia 
matrix  of  the  base  in  the  inertial  frame  (70) .  A  series  of  n  inertia  matrices  in  the  inertial 

frame  (one  for  each  link)  are  calculated  based  on  Equation  22. 
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(22) 


3.  Mass  Matrices 

The  link  mass  matrices  (M.)  and  the  base  mass  matrix  (M0)  takes  the  general 
form  shown  in  Equation  23  and  takes  into  account  the  inertia  (7)  and  mass  (m)  properties 
of  each  link.  In  the  figure  below,  133  represents  a  3x3  identity  matrix  [48]. 


M  = 


I  0 
0  ml 


3,3 


(23) 

From  a  physical  perspective,  the  mass  and  inertia  properties  of  the  links  and  of  the 
base  effect  the  twisting  of  the  joints.  The  mass  composite  body  matrices  of  the  links 
(M  m)  take  into  account  these  effects  (encapsulated  in  the  twist  propagation  matrices)  by 
first  computing  the  effects  at  the  nth  joint  and  conducting  a  backwards  recursion.  When 
the  recursion  has  accounted  for  all  links,  Mm  is  the  end  result  (Equation  24).  The  mass 


composite  body  matrix  of  the  base  (M0)  is  computed  using  the  Bj0  twist  propagation 

matrix  and  the  mass  composite  body  matrix  produced  by  the  recursion  (Equation  25) 
[48].  The  code  for  the  function  used  to  conduct  these  calculations  is  given  in  Appendix 
M:  MCB  Serial.m. 


Mm=Mm,i+BliMm.+lBiji 


M0=M0+Bf0MmlBi0 


(24) 

(25) 


4.  Generalized  Inertial  Matrices 

With  M0,Mm,Bij,Bi0,P0,pm,  the  generalized  inertia  matrix  of  the  base  (H0)  and 

the  generalized  coupling  inertia  matrix  of  the  manipulator  and  the  base  (H0m)  can  be 
computed.  The  script  that  accomplishes  this  calculation  is  given  in  Appendix  N: 
GIMSerial.m.  The  calculation  of  H0  follows  the  matrix  multiplication  given  in  Equation 
26  [48].  Similarly,  the  coupling  inertia  matrix,  which  accounts  for  the  twists  of  the  joints, 
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the  inertias  and  masses  of  the  links,  and  how  these  affect  the  inertia  of  the  base- 
manipulator  system  is  given  in  Equation  27  [48], 


The  goal  of  calculating  inertias,  mass  composite  body  matrices,  and  generalized 
inertia  matrices  is  to  determine  the  base-manipulator’s  state  (q) ,  which  consists  of  the 
base  state  vector  and  the  manipulator  state  vector  as  shown  in  Equation  28. 


(28) 

The  time  derivative  of  the  base’s  state  vector  ( q0 )  is  calculated  by  Equation  29, 
which  is  based  on  the  conservation  of  momentum  principle  and  includes  previously 
calculated  inertia  matrices  [48].  The  manipulator  joint  velocities  ( qm )  are  calculated  by 

subtracting  the  initial  measured  position  of  each  joint  from  the  final  measured  position  of 
each  joint  and  dividing  by  the  time  of  the  maneuver  (Equation  30).  The  calculation  of  q  is 
accomplished  by  the  function  in  Appendix  O:  q_dot_fun.m. 

Since  the  state  of  the  base  is  only  a  function  of  the  manipulator  path,  the 
experimentation  can  exploit  a  non-holonomic  constraint.  That  is,  since  only  one  joint  is 
moved  at  a  time,  the  time  over  which  the  movement  occurs  is  irrelevant;  the  system  will 
achieve  the  same  position  whether  the  maneuver  takes  one  or  one  hundred  seconds.  For 
the  purposes  of  the  MATLAB  calculations,  all  maneuver  times  (At)  were  set  to  be  five 
seconds.  The  derivative  is  simply  integrated  to  obtain  the  state  vector  of  the  system  ( q ) 
and  the  components  of  (q0x,qijy,q()0)  are  used  in  the  calibration  proper.  See  Appendix  P: 
qOmaneuver.m  for  the  integration  function. 
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(29) 


4o  H0  ^ Onttm 

6.  Inducing  Variation,  the  Dynamic  Calibration  Proper 

As  in  the  kinematics  calibration,  in  the  dynamics  calibration,  small  variations  are 
applied  to  the  parameters  of  interest — in  this  case,  the  b  vector  and  the  inertia  I.  These 
small  variations  are  passed  through  the  dynamics  model.  These  partial  differentials  are 
used  to  form  the  dynamics  calibration  matrix,  which,  like  the  kinematics  calibration 
matrix  is  denoted  as  O  .  In  this  case  however,  ®  depends  upon  how  much  the  outputs  of 
the  dynamics  model  (d )  change  with  changes  in  the  parameters  of  interest  (Equation  31). 


®  =  [Sd/Sb  Sd/Sl] 

Figure  63.  Dynamic  Calibration  Matrix. 


(31) 


The  dynamics  model  (see  Appendix  P:  qOmaneuver.m)  integrates  the  joint 
positions  to  ultimately  predict  the  coordinates  of  the  system  state  vector  in  the  inertial 
frame.  As  in  the  kinematics  calibration,  ®  and  the  matrix  of  nominal/measured 
differences  (Ar)  are  used  to  construct  AC  ,  and  a  least-squares  method  is  used  to  refine 
the  parameters.  Ultimately,  the  dynamics  calibration  provided  insight  into  how  much  the 
link  inertias  (/ )  and  the  b  vectors  varied  between  theoretical  prediction  and  experimental 
measurement. 

F.  ANALYSIS 

Achieving  a  valid  least-squares  solution  to  the  kinematics  and  then  the  dynamics 

calibration  defined  the  geometry  and  mass  properties  of  the  base-manipulator  system.  In 

the  planar  case  with  revolute  joints,  DH  parameter  a  for  the  links  was  calculated  to  be 

0.382532  m  for  Links  1-3  and  slightly  shorter  for  Link  4.  This  conclusion  makes  sense 

because  Link  4  is  slightly  shorter  than  the  other  links  due  to  the  lack  of  an  attached  joint 

or  end  effector  (Figure  64).  Also,  while  a  deliberate  effort  was  made  to  initially  position 

all  joints  at  a  zero  angle,  the  calibration  process  showed  that  the  joints  were  actually 
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slightly  offset.  Note  that  Link  l’s  joint  offset  is  0.00  degrees,  while  the  offset  of  the  base 
(L0)  is  18.21  degrees.  Since  Joint  1  is  attached  to  the  base,  it  could  equivalently  be  said 
that  the  offset  of  the  base  is  0.00  degrees  while  the  offset  of  Joint  1  is  18.21  degrees. 
Overall,  the  mean  position  residual  of  the  system  was  thousandths  of  a  meter,  and  the 
mean  angular  residual  was  on  the  order  of  hundredths  of  a  degree — indicators  that  the 
kinematic  calibration  was  successful. 


Link  1  DH  a:  0.382532  m 

Link  2  DH  a:  0.382532  m 

Link  3  DH  a:  0.382532  m 

Link  4  DH  a:  0.335136  m 

Link  1  Joint  Offset:  0.000000  deg 

Link  2  Joint  Offset:  5.801469  deg 

Link  3  Joint  Offset:  -3.402930  deg 

Link  4  Joint  Offset:  -1.391795  deg 

L0  to  Jl  parameters  [x,y,  theta]:  0.205857  m,  0.015531  m, 
18.211043  deg. 

position  residuals  [mean, std] :  0.007598,  0.007059 
theta  residuals  [mean, std]:  0.014948,  0.683483  deg. 


Figure  64.  MATLAB  Results  of  the  Kinematics  Calibration. 

The  dynamics  calibration  did  not  provide  as  precise  a  solution  as  the  kinematics 
calibration.  The  inertias  of  the  links  were  ah  calculated  to  be  the  same,  and  the  b  vectors, 
with  the  exception  of  the  shorter  Link  4  were  also  determined  to  be  the  same  (Figure  65). 
The  mean  and  standard  deviation  of  the  data  set,  however,  were  considerably  larger  than 
seen  in  the  kinematics  portion.  This  decrease  in  precision  is  partially  due  to  the  almost 
imperceptible  motion  of  the  base  due  to  Link  4’s  motion,  and  sensitivity  of  the  system  to 
residual  motion  after  the  cessation  of  movement  commands.  During  experimentation,  it 
was  noticed  that  the  system  tended  to  drift  after  it  should  have  come  to  a  stable  stop, 
possibly  due  to  a  misaligned  air-pad,  a  piece  of  air  tubing  dragging  on  the  surface  of  the 
table,  or  slight  imperfections  on  the  surface  of  the  monolith. 
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Link  1  I 
Link  2  I 
Link  3  I 
Link  4  I 
Link  1  b 
Link  2  b 
Link  3  b 
Link  4  b 
position 


0.  034  634  kg  mA2 
0.  034  634  kg  mA2 
0.  034634  kg  mA2 
0.  034634  kg  mA2 
0.  191266  m 
0.  191266  m 
0.  191266  m 
0. 167568  m 

residuals  [mean,std]: 


-0.685248, 


1.549964  deg. 


Figure  65.  MATLAB  Outputs  of  the  Dynamic  Calibration. 
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VI.  CONCLUSION 


A.  SUMMARY  OF  WORK 

This  thesis  built  upon  several  years  of  work  at  the  Naval  Postgraduate  School 
Spacecraft  Robotics  Laboratory  and  took  advantage  of  an  established  laboratory 
environment  with  a  state-of-the-art  motion  tracking  system,  granite  monolith,  and 
already-constructed  Floating  Spacecraft  Simulator  (FSS)  platform.  The  initial  link  design 
was  improved  by  adopting  an  all  commercial-off-the-shelf  hardware  components, 
changing  the  communications  architecture  to  one  that  uses  Wi-Fi,  and  coding  more 
effective  software  for  the  processing  of  the  system’s  data.  The  initial  six  months  of  the 
effort  centered  around  gaining  an  understanding  of  the  previous  work  and  the  integration 
of  the  new  hardware  components  and  software.  Glitches  in  software  development  caused 
significant  delay  but  were  ultimately  overcome. 

With  the  second  link  completed,  the  design  improvements  were  retrofitted  onto 
the  original  link,  and  the  third  and  fourth  links  were  constructed  in  relatively  little  time. 
With  the  four-link  configuration  constructed  and  attached  to  the  FSS,  the  kinematic  and 
dynamic  calibration  processes  began.  MATLAB  and  Simulink  tools,  which  became  the 
Spacecraft  Robotics  Toolkit  (SPART),  were  developed  and  validated  via  table-top 
experimentation.  The  work  for  this  thesis  contributed  to  three  conference  papers.  Two 
were  presented  at  the  6th  International  Conference  on  Astrodynamics  Tools  and 
Techniques  (ICATT),  and  an  abstract  for  the  third  has  been  accepted  for  the  American 
Institute  of  Aeronautics  and  Astronautics’  (AIAA)  Space  and  Aeronautics  Forum  and 
Exposition  in  September  2016  [23],  [42],  [49].  Additionally,  a  patent  application  is  being 
pursued. 

B.  FUTURE  WORK 

Due  to  the  modular  nature  of  the  manipulator  links,  future  research  can  take  a 
number  of  different  directions.  In  the  near  tenn,  however,  work  will  likely  continue  with 
the  four-link  manipulator  configuration. 
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(1)  How  Small  is  Small? 

Throughout  the  literature  on  the  subject,  the  term  “small  spacecraft”  is  used  when 
describing  a  spacecraft  whose  motion  is  significantly  dynamically  coupled  with  the 
motion  of  the  manipulator.  As  discussed  in  the  introduction,  the  ratio  of  the  base 
mass/inertia  to  the  manipulator  mass/inertia  are  useful  measures  of  how  heavily  the 
dynamics  are  likely  to  couple.  There  is,  however,  no  apparent  effort  to  quantify  at  what 
ratios  the  dynamics  become  “significantly”  coupled.  How  big  does  the  manipulator  have 
to  be  to  affect  the  base  dynamics?  How  small  does  the  manipulator  have  to  be  for  its 
effects  to  be  ignored?  With  the  tools  developed  in  the  SRL,  this  topic  could  be 
investigated  via  simulation  and  table-top  experimentation. 

(2)  Explore  Manipulator  Control  Algorithms 

The  intent  of  the  kinematics  and  dynamics  calibration  (and  all  the  proceeding 
hardware  and  software  development)  was  to  put  in  place  the  pieces  for  a  controllable 
manipulator.  As  a  stepping  stone,  efforts  to  control  the  manipulator  not  from  a  command 
laptop  (as  done  in  the  experimentation  for  this  thesis)  but  from  the  FSS’s  onboard 
computer  are  underway.  Other  areas  ripe  for  investigation  include  the  most  effective 
manner  in  which  to  employ  a  manipulator  with  respect  to  time  or  power  usage;  these  are 
two  areas  in  which  optimal  control  theory  could  be  explored. 

The  current  investigation  focused  on  a  four-link  serial  manipulator,  but  the 
modularity  of  MARSMAN  allows  for  exploration  of  different  topologies.  For  example, 
with  the  in-house  additive  manufacturing  capability,  a  serial  manipulator  that  branches 
into  multiple  arms  could  be  constructed.  Further,  closed-tree  topologies  such  as  two  two- 
link  manipulators  working  in  concert  as  a  pincer  could  be  investigated.  These  topologies 
are  significantly  more  difficult  to  analyze  and  require  the  employment  of  differential 
algebraic  equations  (DAE).  Whatever  the  unique  configuration,  different  configurations 
of  the  modular  links  will  require  adaptations  of  the  kinematics  and  dynamics  calibration 
and  present  their  own  control  challenges 


90 


(3)  Construct  and  Integrate  an  End  Effector 

This  project  focused  on  the  construction  of  the  manipulator  links  themselves,  and 
the  mathematical  modeling  assumed  a  notional  end  effector  was  attached.  An  end 
effector  is  necessary  for  proximity  operations  that  aim  to  capture  a  target  FSS.  A  simple 
end  effector  could  consist  of  a  Velcro  surface  mounted  to  both  the  target  vehicle  and  the 
end  of  the  manipulator.  More  complex  end  effectors,  like  grippers,  provide  additional 
opportunities  for  hardware  construction  and  software  integration.  Yale  University’s 
OpenHand  Project  provides  computer  aided  drafting  (CAD)  files  for  a  variety  of 
manipulators  that  can  be  assembled  via  additive  manufacturing.  The  tendons  for  the 
gripper  are  constructed  by  pouring  resin  into  a  3-D  printed  mold  [50].  With  a  gripper, 
rendezvous  and  capture  problems  can  become  an  active  area  of  research. 


Figure  66.  Sample  End  Effector  from  the  Yale  OpenHand  Project.  Source:  [50]. 

(4)  Rendezvous  and  Capture  Operations 

With  viable  control  algorithms  and  an  end  effector,  rendezvous  and  capture 
operations  become  possible.  Initial  attempts  may  consider  capturing  a  stationary 
spacecraft  with  the  eventual  goal  of  de -tumbling  a  spinning  FSS,  perhaps  while 


91 


maintaining  visual  observation  via  a  camera  mounted  on  the  base  spacecraft.  This  line  of 
research  is  synergistic  with  the  current  research  efforts  at  the  SRL  for  FSS-to-FSS 
docking.  From  a  guidance,  navigation,  and  control  (GNC)  perspective,  this  problem 
becomes  particularly  challenging  because  it  requires  not  only  control  of  the  manipulator 
itself  but  also  control  of  the  base  spacecraft  via  its  thrusters  and  reaction  wheels. 

(5)  Hardware  Upgrades 

The  Arduino  Wi-Fi  shield  is  no  longer  in  production.  As  components  fail,  they 
will  need  to  be  replaced  with  a  comparable  capability.  The  Raspberry  Pi  microcontroller 
has  the  potential  to  replace  the  Arduino  Wi-Fi  shield,  the  Arduino  Due,  and  the  RS232 
shield  because  it  can  serve  as  microcontroller,  create  its  own  ad  hoc  Wi-Fi  network,  and 
provide  the  necessary  connection  ports  to  the  motor  driver.  The  creation  of  the  ad  hoc 
Wi-Fi  network  will  allow  for  the  links,  the  FSS,  and  a  telemetry  laptop  to  communicate 
directly  in  a  more  realistic  and  reliable  manner.  With  this  capability,  the  D-link  router  can 
be  removed  from  the  communications  architecture.  Further,  the  Raspberry  Pi  allows  for  a 
simplified  interface  to  control  the  end  effector  and  the  possibility  of  attaching  a  small 
camera  for  use  in  proximity  operations. 

(6)  The  Self-Assembling  Manipulator 

Apart  from  organic  propulsion,  the  current  design  of  the  manipulator  links  is  such 
that  they  are  very  nearly  miniature  FSSs.  With  modification  to  the  current  modular 
design,  each  link  could  be  made  to  operate  independently,  and  the  links  themselves  could 
rendezvous  to  form  a  workable  manipulator  that  could  then  be  used  to  perform  a  useful 
task. 

(7)  A  Real  Manipulator  Spacecraft 

The  current  base-manipulator  system  is  a  test-bed  configuration  never  intended  to 
fly  in  space.  While  the  development  of  a  flight-worthy  base-manipulator  system  is 
outside  the  current  scope  of  the  SRL’s  research  efforts,  the  development  of  such  a 
concept  could  easily  provide  research  material  for  students  in  structures,  thermal  systems, 
power  systems,  and  GNC.  The  development  of  such  a  spacecraft  would  be  a  worthy  of 
exploration  during  NPS’s  capstone  design  sequence. 
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C.  RESEARCH  SIGNIFICANCE 

The  research  conducted  for  this  thesis  is  significant  in  several  ways.  As 
mentioned  previously,  the  initial  development  of  a  modular  robotic  manipulator  link  in 
2014  was  the  first  of  its  kind.  To  the  knowledge  of  the  author,  the  subsequent  hardware 
and  software  improvements  and  the  construction  of  the  four-link  manipulator  from 
modular  links  is  unique  in  academia.  The  kinematic  and  dynamic  models,  elucidated  by 
SRL  writings  such  as  [48],  was  implemented  via  MATLAB  and,  along  with  the 
calibration  code,  made  publicly  available  to  anyone  wishing  to  investigate  spacecraft  with 
attached  manipulators — another  first-of-its-kind  effort  to  share  knowledge  with  the  larger 
space  robotics  community.  The  research  work  over  the  past  year  has  contributed  to  two 
published  conference  papers;  a  third  conference  paper  abstract  based  on  the  work  has 
been  accepted  for  the  fall  of  2016.  Finally,  the  manipulator  is  being  used  to  teach 
graduate  students  robotics  and  multi-body  mechanics  at  NPS. 

Ultimately,  this  project  implemented  the  highly  non-linear  dynamical  model  of  a 
small  spacecraft  with  an  attached  robotic  manipulator  consisting  of  multiple  modular 
links.  The  concept  was  validated  through  a  table-top  experimentation  campaign  that 
proved  the  concept  of  controlling  multiple  links  independently  to  complete  the  kinematic 
and  dynamic  calibrations.  The  experimental  framework  will  allow  further  exploration 
into  spacecraft/robotic  mechanics,  control  theory,  and  operational  concepts  ranging  from 
docking  and  capture  maneuvers  to  spacecraft  servicing.  The  beauty  of  the  MARSMAN 
system  is  that  a  new  piece  of  hardware  does  not  need  to  be  funded  and  built  each  time  a 
new  line  of  inquiry  appears.  The  modular  links  can  be  reconfigured  for  any  number  of 
mission  scenarios  that  may  be  of  interest  to  civil,  military,  or  commercial  space  entities. 
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APPENDIX  A.  SPARKFUN  HX711  LOAD  CELL  AMPLIFIER  OPEN- 

SOURCE  HEADER  FILE  [51] 


#ifndef  HX711_h 
#define  HX711_h 


#if  ARDUINO  >=  100 
#include  "Arduino.h" 
#else 

#include  "WProgram.h" 
#endif 


class  HX711 

{ 

private : 

byte  PD_SCK; 

Input  Pin 

byte  DOUT; 
byte  GAIN; 
long  OFFSET; 
float  SCALE; 
kg,  ounces,  whatever 


/ /  Power  Down  and  Serial  Clock 

//  Serial  Data  Output  Pin 
//  amplification  factor 
//  used  for  tare  weight 
//  used  to  return  weight  in  grams. 


public : 

// 


factor 


define  clock  and  data  pin. 


channel , 


and  gain 


//  channel  selection  is  made  by  passing  the 
appropriate  gain:  128  or  64  for  channel  A,  32  for  channel  B 
//  gain:  128  or  64  for  channel  A;  channel  B  works 
with  32  gain  factor  only 

HX711 (byte  dout,  byte  pd_sck,  byte  gain  =  128); 


virtual  ~HX711 ()  ; 


//  check  if  HX711  is  ready 

//  from  the  datasheet:  When  output  data  is  not 
ready  for  retrieval,  digital  output  pin  DOUT  is  high. 
Serial  clock 

//  input  PD_SCK  should  be  low.  When  DOUT  goes  to 
low,  it  indicates  data  is  ready  for  retrieval. 

bool  is_ready(); 

//  set  the  gain  factor;  takes  effect  only  after  a 
call  to  read() 
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//  channel  A  can  be  set  for  a  128  or  64  gain; 
channel  B  has  a  fixed  32  gain 

//  depending  on  the  parameter,  the  channel  is 
also  set  to  either  A  or  B 

void  set_gain (byte  gain  =  128); 

//  waits  for  the  chip  to  be  ready  and  returns  a 

reading 

long  read ( ) ; 

//  returns  an  average  reading;  times  =  how  many 
times  to  read 

long  read_average (byte  times  =  10); 

//  returns  (read_average ( )  -  OFFSET),  that  is  the 

current  value  without  the  tare  weight;  times  =  how  many 
readings  to  do 

double  get_value (byte  times  =  1); 

//  returns  get_value()  divided  by  SCALE,  that  is 
the  raw  value  divided  by  a  value  obtained  via  calibration 
//  times  =  how  many  readings  to  do 
float  get_units (byte  times  =  1); 

//  set  the  OFFSET  value  for  tare  weight;  times  = 
how  many  times  to  read  the  tare  value 
void  tare (byte  times  =  10); 

//  set  the  SCALE  value;  this  value  is  used  to 
convert  the  raw  data  to  "human  readable"  data  (measure 
units ) 

void  set_scale  ( float  scale  =  l.f); 

//  set  OFFSET,  the  value  that's  subtracted  from 
the  actual  reading  (tare  weight) 

void  set_offset (long  offset  =  0); 

//  puts  the  chip  into  power  down  mode 
void  power_down ( ) ; 

//  wakes  up  the  chip  after  power  down  mode 
void  power_up(); 

}; 

#endif  /*  HX711_h  */ 
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APPENDIX  B.  SPARKFUN  HX711  LOAD  CELL  AMPLIFIER  OPEN- 
SOURCE  C++  SOURCE  CODE  FILE  [51] 


#include  <Arduino.h> 

#include  <HX711.h> 

HX7 1 1 : : HX7 1 1 (byte  dout,  byte  pd_sck,  byte  gain)  { 
PD_SCK  =  pd_sck; 

DOUT  =  dout; 

pinMode (PD_SCK,  OUTPUT) ; 
pinMode (DOUT,  INPUT); 

set_gain (gain) ; 

} 

HX711 : : -HX711 ()  { 


bool  HX7 11:: is_ready ( )  { 

return  digitalRead (DOUT )  ==  LOW; 

} 

void  HX7 1 1 : : set_gain (byte  gain)  { 
switch  (gain)  { 


case 

128  : 

GAIN  = 

break; 

i; 

// 

channel 

A, 

gain 

factor 

128 

case 

64  : 

GAIN  = 
break; 

3; 

// 

channel 

A, 

gain 

factor 

64 

case 

32  : 

GAIN  = 
break; 

2; 

// 

channel 

B, 

gain 

factor 

32 

} 

digitalWrite (PD_SCK,  LOW) ; 
read  ( )  ; 


long  HX7 1 1 : : read ( )  { 

/ /  wait  for  the  chip  to  become  ready 
while  ( ! is_ready ( )  )  ; 
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byte  data  [  3 ] ; 


//  pulse  the  clock  pin  24  times  to  read  the  data 
for  (byte  j  =  3;  j — ;  )  { 

for  (char  i  =  8;  1--;)  { 

digitalWrite (PD_SCK,  HIGH); 

bitWrite (data [ j ] ,  i,  digitalRead (DOUT) ) ; 

digitalWrite (PD_SCK,  LOW) ; 

} 

} 

/  /  set  the  channel  and  the  gain  factor  for  the  next 
reading  using  the  clock  pin 

for  (int  i  =  0;  i  <  GAIN;  i++)  { 

digitalWrite (PD_SCK,  HIGH); 
digitalWrite (PD_SCK,  LOW) ; 

} 

data[2]  A=  0x80; 

return  ( (uint32_t)  data[2]  <<  16)  |  ( (uint32_t) 

data[l]  <<  8)  |  (uint32_t)  data[0]; 

} 

long  HX7 1 1 : : read_average (byte  times)  { 
long  sum  =  0; 

for  (byte  i  =  0;  i  <  times;  i++)  { 

sum  +=  read ( )  ; 

} 

return  sum  /  times; 

} 

double  HX7 1 1 : : get_value (byte  times)  { 

return  read_average (times )  -  OFFSET; 

} 

float  HX7 1 1 : : get_unit s (byte  times)  { 
return  get_value (times )  /  SCALE; 

} 

void  HX7 1 1 :: tare (byte  times)  { 

double  sum  =  read_average (times)  ; 
set_offset (sum) ; 

} 

void  HX7 1 1 :: set_scale ( float  scale)  { 
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SCALE  =  scale; 

} 

void  HX7 1 1 :: set_off set ( long  offset)  { 
OFFSET  =  offset; 

} 

void  HX7 1 1 : : power_down ( )  { 

digitalWrite (PD_SCK,  LOW) ; 
digitalWrite (PD_SCK,  HIGH); 

} 

void  HX7 1 1 : : power_up ( )  { 

digitalWrite (PD_SCK,  LOW) ; 

} 
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APPENDIX  C.  ONBOARDLINK.INO 


//  Arduino  On-board  Software  controlling  a  manipulator  link  (OnBoardLink.ino) 
//  Sends  telemetry  and  receives  commands  via  TCP/UDP. 

//  Developed  by  Dr.  Josep  Virgili-Llop  and  CPT  Jerry  Drew 

//  Through  UDP  the  manipulator  sends: 

//  *  Position  in  rad. 

//  *  Angular  velocity  in  rad/s. 

//  *  Joint  torque  in  Nm 

// 

//  The  manipulator  accepts  velocity  commands  in  rad/s. 

// 

//—  CODE  — // 

//Include  libraries 
#include  <SPI.h> 

#include  <WiFi.h> 

#include  <WiFiUdp.h> 

#include  "HX71  l.h"  //for  load  cell  amp  to  read  torque 

//Main  loop  time  in  ms 
#define  LOOP  TIME  100  //  10  Hz 

//Wireless  Parameters 

int  stat  w  =  WL  IDLE  STATUS; 

#dellnc  ssid  "dlink_srl"  //  Network  SSID  (name) 

#define  FSS_IP  ”192.168.0.133"  //IP  of  the  FSS 
#define  localPort  4097  //  Local  port  to  listen  on 

//FSS  ports  (will  be  assigned  depending  on  mac  address) 

#define  FSS_PORT_A  25010 

#define  FSS_PORT_B  25020 

#define  FSS_PORT_C  25030 

#define  FSS_PORT_D  25040 

byte  mac[6];  //Holds  Wi-Fi  shied  mac  address 

int  FSS_port=  FSS  PORT  A;  //Holds  FSS  port 

//Mac  addresses  of  Wifi  shields  (in  reverse!) 

#define  MAC_A  0xbl,0xc5, 0x0 1,0x0e,0xc4, 0x78 
#define  MAC_B  0x7a,0xfb, 0x0 1,0x0e,0xc4, 0x78 
#define  MAC_C  0x49, 0xc5, 0x0 1,0x0e,0xc4, 0x78 
#define  MAC_D  0x47,0xc5, 0x0 1,0x0e,0xc4, 0x78 
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#define  calibration_factor  1 .0 

#define  DOUT  8  //Torque  sensor  DOUT  pin  # 

#define  CLK  9  //Torque  sensor  CLK  pin  # 

//Motor  Parameters 

#define  POS_Conversion  624339  //  Conversion  counts  per  radian 
#define  VEL_Conversion  624339  //  Conversion  counts/s  to  radians  per  second 
#deline  CUR_Conversion  100  //  Conversion  from  driver  response  to  Amperes 
#define  COUNT_OFFSET  0  //  Count  offset  to  0  degrees. 

//Joint  limits 

#define  POS_LIMIT  1.4835  //  85  degrees 
//UDP  packet  buffer 

byte  packetBuffer[255];  //  Buffer  to  hold  incoming  packet 

//Receiver  buffer  for  Serial  to  driver 
String  Driver  String; 

//Create  Wi-Fi  UDP  class 
WiFiUDP  Udp; 

//  Definition  of  the  data  class  that  will  hold  the  data  to  reveieve  and  send 
typedef  union  { 
float  floatPrec; 
byte  binary [4]; 

}  binaryFloat; 

//Loop  variables 

binaryFloat  Pos,  Velocity,  Torque,  CS;  //Data  variables 
//Torque  sensor  sariable 

HX711  scale(DOUT,  CLK);  //defining  a  variable  of  the  class  HX711  that  is  defined  in 
the  X7 1 1  .h  library 

//—  SETUP  — // 
void  setup()  { 

//Initialize  serial  (debug) 

Serial. begin(9600); 

//Initialise  serial  port  with  Driver 
Serial2.begin(9600); 

Serial2.setTimeout(20);  // 

Serial.println("Serial  to  Driver  initialized"); 
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//Wait  5  s  to  allow  Driver  power-up 
delay(5000); 


//Initialize  Driver 
DriverSetup(); 

//Initialize  Wifi 
WifiSetupO; 

//Initialize  UPD  listening 
UDPSetupO; 

//Torque  Setup 

Serial.println("Initializing  Torque  Sensor"); 

scale. set_scale(calibration_factor);  //This  value  is  obtained  by  using  the 
SparkFun_HX7 1 1  Calibration  sketch 

scale.tare();  //Assuming  there  is  no  weight  on  the  scale  at  start  up,  reset  the  scale  to  0 
Serial.println("Initalization  Complete"); 

} 


//—  MAIN  LOOP  — // 
float  command=0; 
void  loop()  { 

//Initial  loop  time 
int  t  init  =  millis(); 

//Recieve  velocity  commands 
if  (Udp.parsePacket()){ 

//Read  command 
command= VelC  ommand() ; 

//Within  limits  ->  execute  motion 

if  (abs(Pos.floatPrec)<POS_LIMIT)  {execute(command);} 

else  if  (Pos.floatPrec>POS_LIMIT  &  command<0)  {execute(command);} 

else  if  (Pos.floatPrec<-POS_LIMIT  &  command>0)  {execute(command);} 

} 

//Check  if  the  joint  limits  are  exceeded 

if  (Pos.floatPrec>POS_LIMIT  &  Velocity. floatPrec>0)  {execute(O);}  //Exceeded  limits 
->  stop  motion 

else  if  (Pos.floatPrec<-POS_LIMIT  &  Velocity. floatPrec<0)  {execute(O);}  //Exceeded 
limits  ->  stop  motion 
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//Get  Torque 

float  reading  =  scale. read_average(l); 

Torque. floatPrec  =  (reading-8388608)/487848. 1*0.33; 

//Get  joint  position 
Pos.floatPrec=GetPos(); 

//Get  joint  velocity 

Velocity. floatPrec=GetVelocity(); 

//Compute  checksum 

CS. floatPrec  =  100-(Torque. floatPrec*  100+Pos. floatPrec*  100+Velocity. floatPrec*  100); 

//Send  data  via  UDP 
Udp.beginPacket(FSS_IP,  FSS_port); 

//Udp.write(Time.binary,4); 

Udp.write(Pos. binary, 4); 

Udp.write(Velocity  .binary  ,4); 

Udp.write(Torque.binary,4); 

//Udp.write(Current.binary,4); 

Udp.write(CS.binary,4); 

int  Udp  send  =  Udp.endPacket(); 

//Print  all  the  variables  in  a  single  line 
Serial.print(" Angle:  "); 

Serial.print(Pos.  floatPrec*  1 80/3 . 14 1 5,4); 

Serial.print("deg,  "); 

Serial.print(" Velocity:  "); 

Serial.print(Velocity. floatPrec*  1 80/3 . 14 1 5,4); 

Serial.print("deg/s,  "); 

Serial.print("Torque:  "); 

Serial.print(Torque. floatPrec, 4); 

Serial.print("Nm,  "); 

Serial.print("UDP  Send:  "); 

Serial  .print(U  dp_send) ; 

Serial.print(",  Exec  Time:  "); 

Serial.print(millis()-t_init); 

Serial.println("ms"); 

//Wait  until  total  loop  time  has  elapsed  (loops  executes  at  a  constant  rate) 
while  ((LOOP_TIME+t_init)>millis())  { } 

} 


//...  FUNCTIONS  —  // 


void  WifiSetupQ  { 
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//  check  for  the  presence  of  the  shield: 
delay(  1000);  //Wait  for  WiFi  to  boot  up 
if  (WiFi. status()  ==  WL_NO_SHIELD)  { 
Serial.println("WiFi  shield  not  present"); 

//  don’t  continue: 
while  (true); 

} 

String  fv  =  WiFi.firmwareVersion(); 
Serial.print("Wifi  firmware  version:  "); 
Serial.println(fv); 
if  (  fv  !=  "1.1.0") 

Serial.println("Please  upgrade  the  firmware."); 

//  attempt  to  connect  to  Wifi  network: 
while  (  stat_w  !=  WL_CONNECTED)  { 
Serial.print("Attempting  to  connect  to  SSID:  "); 
Serial.println(ssid); 

//  Connect  to  network. 
stat_w  =  WiFi.begin(ssid); 

//Print  result  (for  debugging) 

Serial.print("Wifi  status:  "); 
Serial.println(stat_w); 

//  wait  2  seconds  for  connection: 
delay(2000); 

} 

Serial.println("Connected  to  wifi"); 

//Get  your  shield  mac  address 
WiFi.macAddress(mac); 

//  Assign  FSS  port  based  on  MAC  address 

byte  mac_a[6]  =  {MAC_A}; 

byte  mac_b[6]  =  {MAC_B}; 

byte  mac_c[6]  =  {MAC_C}; 

byte  mac_d[6]  =  {MAC_D}; 

if  (memcmp(mac,mac_a,6)==0)  { 

FSS_port  =  FSSPORTA; 

} 

else  if  (memcmp(mac,mac_b,6)==0)  { 

FSS_port  =  FSSPORTB; 

} 

else  if  (memcmp(mac,mac_c,6)==0)  { 
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FSS_port  =  FSSPORTC; 

} 

else  if  (memcmp(mac,mac_d,6)==0)  { 
FSS_port  =  FSSPORTD; 

} 


//Print  wifi  variables 
printWifiStatus(); 

} 


//Display  WIFI  information  and  get  Mac  address 
void  printWifiStatus()  { 

//  print  the  SSID  of  the  network  you're  attached  to 
Serial.print("SSID:  "); 
Serial.println(WiFi.SSID()); 

//  print  your  WiFi  shield's  IP  address: 

IP  Address  ip  =  WiFi.localIP(); 

Serial.print("IP  Address:  "); 

Serial.println(ip); 

//  print  your  WiFi  shield's  MAC  address: 
Serial.print("MAC:  "); 

Serial.print(mac[5],HEX); 

Serial.print(":"); 

Serial.print(mac[4],HEX); 

Serial.print(":"); 

Serial.print(mac[3],HEX); 

Serial.print(":"); 

Serial.print(mac[2],HEX); 

Serial.print(":"); 

Serial.print(mac[  1  ]  ,HEX); 

Serial.print(":"); 

Seriai.printin(mac[0],HEX); 

//  print  the  target  FSS  port 
Serial.print("FSS  port:  "); 
Serial.println(FSS_port); 


//  print  the  received  signal  strength: 
long  rssi  =  WiFi.RSSIQ; 
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Serial.print("signal  strength  (RSSI):"); 

Serial.print(rssi); 

Serial.println("  dBm"); 

} 

//Setup  the  Driver  to  Velocity  Mode 
void  DriverSetup()  { 

//Set  all  the  variables  required 
Serial.println("Setting  up  Current  Mode."); 

//Set  acceleration  rate 
Serial.println("Setting  acceleration  rate."); 
Serial2.print("s  r0x36  "); 

Serial2.print(50000);  //Acceleration  in  counts/secondA2 
Serial2.print("\n"); 

DriverString  =  Serial2.readStringUntil(V); 
Serial.println(DriverString); 

//Set  decceleration  rate 
Serial.println("Setting  decceleration  rate."); 
Serial2.print("s  r0x37  "); 

Serial2.print(50000);  //Decceleration  in  counts/secondA2 
Serial2.print("\n"); 

DriverString  =  Serial2.readStringUntil(V); 
Serial.println(DriverString); 

//Set  Velocity 

Serial.println("Setting  Velocity"); 

Serial2.print("s  r0x2f  "); 

Serial2.print(0);  //Velocity  in 
Serial2.print("\n"); 

DriverString  =  Serial2.readStringUntil(V); 
Serial.println(DriverString); 

//Enable  Amplifier 
Serial.println("Enable  Amplifier."); 

Serial2.print("s  r0x24  1  l\n"); 

DriverString  =  Serial2.readStringUntil(V); 
Serial.println(DriverString); 

} 

//Get  position  joint  data  from  the  driver 
float  GetPosQ  { 
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//Send  command  to  driver  asking  for  motor  position. 

Serial2.print("g  r0x32\n"); 

//Read  driver  reply 

DriverString  =  Serial2.readStringUntil(V); 

//Remove  the  first  "v" 

DriverString  =  DriverString. substring(2); 

//Conver  it  to  an  angle  in  rad 

return  (DriverString.toFloat()-COUNT_OFFSET)/POS_Conversion; 

} 


//Get  joint  velocity  data  from  the  driver 
float  GetVelocity()  { 

//Send  command  to  driver  asking  for  motor  velocity. 
Serial2.print("g  r0xl8\n"); 

//Read  driver  reply 

DriverString  =  Serial2.readStringUntil(V); 
//Remove  the  first  "v" 

DriverString  =  DriverString. substring(l); 

//Conver  it  to  an  angular  velocity  in  rad/s 
return  DriverString. toFloat()/ 1 0/VEL_Conversion; 

} 

//Get  joint  velocity  data  from  the  driver 
float  GetCurrent()  { 

//Send  command  to  driver  asking  for  motor  current. 
Serial2 .print/ "g  r0x0c\n"); 

//Read  driver  reply 

DriverString  =  Serial2.readStringUntil(V); 
//Remove  the  first  "v" 

DriverString  =  DriverString. substring/ 1); 

//Convert  it  to  a  current  in  Amps 

return  DriverString. toFloatQ/CURConversion; 


} 

//Send  velocity  command  to  driver 
void  execute/float  Velocity)  { 

//Set  Velocity 
Serial2 .print/ "s  r0x2f  "); 

Serial2.print(Velocity*VEL_Conversion*10,0);  //Velocity  in  0.1  counts  per  second 
Serial2.print("\n"); 
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//Read  driver  response 

DriverString  =  Serial2.readStringUntil(V); 

} 


//Initialize  UDP  connection 
void  UDPSetup(){ 

Serial.println("Starting  connection  to  server..."); 

//  if  you  get  a  connection,  report  back  via  serial: 
if  (Udp.begin(localPort)  ==  1)  Serial.println("started"); 
else  Serial.println("failed"); 

} 

//Read  velocity  command  from  UDP 
float  VelCommand()  { 

//Read  velocity  command  from  UDP  stream 
binaryFloat  Velocity; 

Udp.read(Velocity. binary  ,4); 
return  Velocity. floatPrec; 

} 
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APPENDIX  D.  MARSMAN  DRIVER  LOAD.CCX 


14 

1 

95,0, Host  Config  State, 2 : -1 : 0 : 1792 : -1 : -1 : 150 : -1 : -1 : -1 : -1 : - 

87,0, Amp  Family, 3 

40,0, Motor  Type, 48 

89,0, Amp  Max  Voltage, 910 

84,0, Amp  ISense, 1050 

83,0, Amp  Continuous  Current, 300 

82,0, Amp  Peak  Current, 900 

88,0, Amp  Peak  Current  Time, 1000 

0,0, Current  Cp, 55 

1,0, Current  Ci,19 

2 , 0 , Programmed  Current  Command, 0 

19,0, Analog  Input  Scale, 425 

la, 0, Analog  Input  Offset, 0 

21,0, User  Peak  Current  Limit, 425 

22,0, User  Continuous  Current  Limit, 225 

23,0, User  Peak  Current  Time  Limit, 1000 

24,0, Desired  State, 11 

26,0, Analog  Input  Deadband, 0 

27 , 0 , Velocity  Vp, 9984 

28 , 0 , Velocity  Vi, 10000 

2f, 0 , Programmed  Velocity  Command, 0 

31 , 0 , Velocity  Gains  Scalor, 8 

36, 0 , Velocity  Loop  Acceleration  Limit, 500 

37 , 0 , Velocity  Loop  Deceleration  Limit,  500 

39, 0 , Velocity  Fast  Stop  Ramp, 131072 

3a, 0 , Velocity  Loop  Velocity  Limit , 131072000 

3f, 0 , Velocity  Tracking  Time, 100 

3e, 0 , Velocity  Tracking  Window, 3 932 1 6 

41,0, Motor  Manufacturer , Harmonic  Drive  Systems 

42,0, Motor  Model  Number, FHA-8C-30-12S17bE 

43,0, Motor  Units,  0 

44,0, Motor  Inertia, 29000 

46,0, Motor  Brake, 1 

48,0, Motor  Torque  Constant, 3060 

49,0, Motor  Resistance, 108 

4a, 0, Motor  Inductance, 44 

4b, 0, Motor  Peak  Torque, 13000 

4c, 0, Motor  Continuous  Torque, 6900 

4d, 0, Motor  Velocity  Limit , 131072000 

4e,0, Motor  Wiring,! 
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53 , 0 , Brake/Stop  Delay  Time, 0 

54,0, Motor  Brake  Delay  Time, 0 

55,0, Motor  Brake  Activation  Velocity, 0 

56,0, Motor  Back  Emf  Constant, 320 

6a, 0 , Commanded  Current  Ramp, 10000 

70,0, Output  1  Config,  100 : 4000  :  0 

78,0, Input  1  Config, 17 

79,0, Input  2  Config, 0 

7a, 0, Input  3  Config,  0 

7b, 0, Input  4  Config, 0 

7c, 0, Input  5  Config, 0 

7d, 0, Input  6  Config,  0 

80,0, Amp  Model  Number , DEP-0 90-0 9 

86,0, Servo  Period,  4 

8a, 0, Voltage  Sense,  2262 

92,0, Amp  Name, Current 

98 , 0 , Function  Generator  Config, 2 

99, 0, Function  Generator  Frequency,  0 

9a, 0 , Function  Generator  Amplitude, 0 

9b, 0 , Function  Generator  Duty  Cycle, 1000 

a5,0, Input  Configuration  Register, 0 

a7,0, Fault  Mask, 1567 

a8,0, Digital  Command  Config, 0 

a9,0, Digital  Command  Scaling,  1 

ad,  0 , Hardware  Type, 897 

ae, 0, Current  Loop  Offset, 0 
30 , 0 , Posit ion  Pp,1000 

33 , 0 , Posit ion  Vff, 16384 
34 , 0 , Posit ion  Aff , 0 
45,0, Motor  Pole  Pairs, 5 
4f,0, Motor  Hall  Offset, -60 
50,0, Motor  Hall  Type,  0 
52,0, Motor  Hall  Wiring,  0 

5f, 0 , Velocity  Loop  Output  Filter, 8448 : 200 : 0 : 775 : 1550 : 775 

12774 : 32763 : 5813 

60,0, Motor  Encoder  Type, 14 

61,0, Motor  Encoder  Units,  0 

62,0, Motor  Encoder  Counts,  131072 

63,0, Motor  Encoder  Resolution,  1 0 0 0 

64,0, Motor  Electrical  Distance, 100000 

65,0, Motor  Encoder  Direction,  1 

6c, 0 , Posit ion  Capture  Control  Register, 5 

71,0, Output  2  Config, 100 : 44007f : 0 

8e,0,Amp  Ref  Scale, 11220 

af, 0,Amp  Options  Register, 2 
bl , 0 , Increment  Rate,0 
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b2 , 0 , Commutation  Mode, 6 

ba,  0 , Posit ion  Following  Error  Limit, 131072 

bb,  0 , Posit ion  Following  Warning  Limit, 65536 
be, 0 , Posit ion  Tracking  Window, 32768 

bd,  0 , Posit ion  Tracking  Window  Time, 10 
c8 , 0 , Trajectory  Profile  Mode, 0 

ca,  0 , Trajectory  Position  Command, 1 

cb,  0 , Trajectory  Max  Velocity, 32768000 

cc,  0 , Trajectory  Max  Accel, 1310720 

cd,  0 , Trajectory  Max  Decel, 1310720 
cf , 0 , Trajectory  Abort  Decel, 6553600 
f0,0, Input  1  Debounce  Time, 0 

fl,0, Input  2  Debounce  Time, 0 

f2,0, Input  3  Debounce  Time, 0 

f3,0, Input  4  Debounce  Time, 0 

f4,0, Input  5  Debounce  Time, 0 

f5,0, Input  6  Debounce  Time, 0 

114 , 0 , Velocity  Vi  Drain,  0 

10c, 0, Network  Heart  Beat  Time, 0 

lOd, 0 , CANopen  Node  Guarding  Time,0 

lOe, 0 , CANopen  Node  Guarding  Life  Time  Factor, 0 

109, 0, Camming  Master  Velocity,  0 

105, 0, Camming  Configuration, 4352 

10 6, 0 , Camming  Forward  Delay, 0 

107, 0, Camming  Reverse  Delay, 0 

104,0, Phase  Init  Config, 0 

be,  0, Software  Limit  Deceleration, 655360 
e4,0, Phase  Init  Current, 425 

e5,0, Phase  Init  Time, 400 

58,0, Gear  Ratio, 65537 

b6,0,PWM  Deadband, 1 0 0 0 

ea,0, Detent  Correction  Gain, 0 

57,0, Micro  Steps  Per  Rev.,0 

59,0, Hall  Velocity  Shift, 0 

5a, 0, Multi  Mode  Port  Configuration,! 

5b, 0 , Posit ion  Encoder  Resolution, 4000 
5c, 0 , Posit ion  Encoder  Direction,  0 
5d, 0 , Posit ion  Encoder  Type, 0 
67,0, Analog  Encoder  Shift,  0 

6b, 0 , Velocity  Loop  Command 

7936:200:0:775:1550:775:-12774:32763:5813 

6f , 0, PWM  Mode,  0 

72,0, Output  3  Config, 0:0:0 

73,0, Output  4  Config, 0:0:0 

7e,0, Input  7  Config, 0 

7f,0, Input  8  Config, 0 


Filter, - 
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b3,0, Analog  Encoder  Scale, 6667 

b8 , 0 , Posit ive  Software  Limit, 10000 

b9, 0 , Negative  Software  Limit, 1000 

bf,0,Home  Current  Delay  Time, 250 

cl, 0, Node  ID  configuration, 1024 

c2,0,Home  Configuration, 512 

c3,0,Home  Velocity  Fast, 3276800 

c4,0,Home  Velocity  Slow, 655360 

c5,0,Home  Accel/Decel, 655360 

c6,0,Home  Offset, 0 

c7,0,Home  Current, 113 

ce, 0, Trajectory  Max  Jerk, 26214400 

d0,0, Input  9  Config, 0 

dl,0, Input  10  Config,  0 

d2,0, Input  11  Config, 0 

d3,0, Input  12  Config, 0 

d8,0, Regen  Resistance,  0 

d9,0, Regen  Continuous  Power,  0 

da,  0, Regen  Peak  Power,  0 

db, 0, Regen  Peak  Power  Time, 0 

el, 0, Regen  Resistor  Model  Number, None 
e3 , 0 , Posit ion  Loop  Gains  Multiplier, 100 
e8,0,uStep  Holding  Current, 0 
e9,0,uStep  Run  To  Hold  Time, 0 
f6,0, Input  7  Debounce  Time, 0 
f7,0, Input  8  Debounce  Time, 0 
f8,0, Input  9  Debounce  Time, 0 
f9,0, Input  10  Debounce  Time, 0 

fa,  0, Input  11  Debounce  Time, 0 

fb, 0, Input  12  Debounce  Time, 0 

103 , 0 , Network  address  Input  Map, 0 
11a, 0, Amp  Scaling  Config,  0 
12 1 , 0 , Network  Options, 0 
123,0, Motor  Position  Wrap  Value, 0 
124,0, Load  Position  Wrap  Value, 0 

125,0, MACRO  Amplifier's  Encoder  Capture  Config, 0 

127,0, Gain  Scheduling  Config, 0 

12a, 0, Motor  Encoder  Options,  301994001 

12b, 0 , Posit ion  Encoder  Options, 1 

12d,0, Analog  Input  Filter, -7936 : 200 : 0 : 775 : 1550 : 775 

12774 : 32763 : 5813 

lOf, 0 , Registration  Offset  For  Pulse  and  Direction, 0 
13c, 0 , Minimum  PWM  Pulse  Width  PWM  Position  Mode, 1000 
13d, 0 , Maximum  PWM  Pulse  Width  PWM  Position  Mode, 2000 
100 , 0 , CANopen  limit  mask, 25364352 
94c, 0, basic  host  cfg, 33 
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APPENDIX  E.  VICON  CALIBRATE.M 


%  Vicon_Calibrate . m  captures  the  VICON  data  and  saves  it  to  an  m-file 
%  Developed  by  multiple  users  within  the  Spacecraft  Robotics  Laboratory 
%  Adapted  by  Dr.  Josep  Virgili-Llop  and  CPT  Jerry  Drew 

%  The  data  is  saved  in  the  following  format: 

%  [X  [m]  ,  Y  [m]  ,  Z  [m] ,  EulerX  [rad],  EulerY  [rad],  EulerZ  [rad].  Time 
[s]  ] 

"6 

%  Needs  the  Vicon  SDK  added  in  the  path  for  it  to  work 
(http : / / www . vicon . com/products/ software/ datastream-sdk) . 

% - Clean  and  Clear - % 

clc 

clear  all 
close  all 

% - Object  Name - % 

%Vicon  objects  names 

obj_name  =  { ' Prime2 ' , ' EndEf f ector ' } ;  %Prime2  is  FSS 


% -  Load  Vicon  Matlab  SDK  - % 

fprintf (  'Loading  Vicon  SDK...\n'  ); 

addpath ( ' C : \Program  Files\Vicon\DataStream  SDK\Win64 \MATLAB ' ) 
Client . LoadViconDataStreamSDK ( )  ; 
fprintf (  'done\n'  ); 

% -  Create  Client  and  connect  to  localhost  - % 

%Create  Client 
ViconClient  =  Client (); 

%Connect  to  localhost  (where  Tracker  is  streaming  the  data) 
HostName  =  ' localhost : 801 ' ; 

fprintf (  'Connecting  to  %s  . . . ' ,  HostName  ) ; 
while  -ViconClient . IsConnected ( ) .Connected 
%  Direct  connection 
ViconClient . Connect (  HostName  ); 
end 

% -  Configure  Vicon  Client  - % 

%  Enable  some  different  data  types 
ViconClient . EnableSegmentData ( ) ; 

ViconClient . EnableMarkerData ( )  ; 

ViconClient . EnableUnlabeledMarkerData ( ) ; 

ViconClient . EnableDeviceData ( )  ; 
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%Check  if  data  types  are  enabled 
fprintf (  'Segment  Data  Enabled:  %d\n', 
ViconClient . IsSegmentDataEnabled ( ) .Enabled  ) ; 
fprintf (  'Marker  Data  Enabled:  %d\n', 
ViconClient . IsMarkerDataEnabled ( ) .Enabled  ) ; 


%  Set  the  streaming  mode 

ViconClient . SetStreamMode (  StreamMode . ClientPull  ); 
ViconClient . SetAxisMapping (  Direction . Forward,  ... 

Direction . Left,  . . . 

Direction. Up  );  %  Z-up 


% - Capture  Data - % 

%A  dialog  to  stop  the  loop 

MessageBox  =  msgbox (  'Stop  Capture',  'Vicon  data  capture'  ); 

%A  dialog  to  capture  data 

CaptureBox  =  msgbox (  'Capture',  'Vicon  data  capture'  ); 

%Initialize  data  structure 

EE_meas=[];  %state  vector  of  end  effector  [3x1] 
qO_meas=[];  %state  vector  of  base  [3x1] 

%  Loop  until  the  message  box  is  dismissed 
while  ishandle (  MessageBox  ) 

%Update  message  box 
drawnow; 

%  Get  frame 

while  ViconClient . GetFrame (). Result . Value  ~=  Result . Success 
end 

for  i=l : length (obj_name) 

%Retrieve  object  values 

Output_Get Segment Global Tran si at ion  = 

ViconClient . GetSegmentGlobalTranslation (  char (ob j_name (i) ) , 
char (ob j_name (i) )  ); 

Output_Get Segment GlobalRot at ionEulerXYZ  = 

ViconClient . GetSegmentGlobalRotationEulerXYZ (  char (ob j_name (i) ) , 
char (ob j_name (i) )  ); 

%Save  data 

data  (i, 1:3)  =  [Output_GetSegmentGlobalTranslation . Translation ( 
1  ) /le3,  . . .  %x 

Output_GetSegmentGlobalTranslation . Translation (  2  )/le3, 

%y 

Output_GetSegmentGlobalRotationEulerXYZ . Rotation (  3  )]; 

%theta  z 


%Display  latest  data 
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if  i==l;  clc;  end 

fprintf  (  ' Object :  %s\n ' , char (ob j_name (i) ) ) ; 
fprintf (' Position  x,y,z:  [%7 . 3f , %7 . 3f ] 

[m]  \n  '  ,  data  (i,  1)  ,  data  (i,  2)  )  ; 

fprintf (' Euler  Angles  :  [ % 7 . 2 f ]  [deg] \n ' , data (i, 3) *180/pi) ; 

end 

%Compute  Manipulator  data 
qO  =  data ( 1 , : ) ; 

EE  =  data (2, : ) ; 


if  not ( ishandle (  CaptureBox  )) 
qO_meas (end+1 , 1:3) =q0  ; 

EE_meas (end+1, 1:3) =EE; 

CaptureBox  =  msgbox (  'Capture',  'Vicon  data  capture'  ); 

end 


end 

% -  Disconnect  and  unload  - % 

%  Disconnect  and  dispose 
fprintf (  '  Disconnect ... \n '  ); 

ViconClient . Disconnect ( )  ; 

%  Unload  the  SDK 

fprintf (  'Unload  SDK...\n'  ); 

Client . UnloadViconDataStreamSDK ( ) ; 

% - Ask  to  save  data - % 

%Generate  filename 

f ilename=sprintf ( ' Vicon_%s_%s . mat ' ,  ' MARSMAN_CAL '  ,  datestr (now,  30 ) ) ; 
uisave ( { ' q0_meas ' , ' EE_meas ' } , filename) ; 
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APPENDIX  F.  KINEMATICS  SERIAL.M 


%  Kinematics_Serial . m  computes  the  kineamtics  of  a  serial  manipulator. 

%  Developed  by  Dr.  Josep  Virgili-Llop 

function 

[RJ,  RL,  r ,  1 , e,  tO ,  tm,  Bij,BiO,PO, pm, TEE] =Kinematics_Serial (RO , rO , qm, qOdot , 
qmdot, data) 

%  Input  -> 

%  RO  ->  Rotation  matrix  from  the  base-spacecraft  to  the  inertial 
frame . 

%  rO  ->  Position  of  the  base-spacecraft  to  the  inertial  frame. 

%  qm  ->  Manipulator  joint  varibles. 

%  qOdot  ->  Base-spacecraft  velocities  [angular  velocity  in  body, 
linear 

%  velocity  in  inertial] . 

%  qmdot  ->  Manipulator  joint  rates. 

%  data  ->  Manipulator  data. 

%  data.n  ->  Manipulator  number  of  joints  and  links. 

%  data. base  ->  Base-spacecraft  data 

%  data . base . T_L0_J1  ->  Homogeneous  transformation  of  the 

first 

%  joint  w.r.t.  the  base-spacecraft. 

%  data. man  ->  Manipulator  data. 

%  data. man (i) . DH  ->  DH  parameters  of  the  ith  joint. 

%  data. man (i) .type  ->  Type  of  joint.  type==0  for  revolute, 

%  otherwise  prismatic. 

%  data. man (i) .b  ->  Vector  from  the  ith  link  to  the  following 

%  joint  i+1 . 

'O 

%  Output  -> 

%  RJ  ->  Joint  3x3  rotation  matrices. 

%  RL  ->  Links  3x3  rotation  matrices. 

%  r  ->  Links  positions. 

%  1  ->  Joints  positions. 

%  e  ->  Joints  rotations  axis. 

%  tO  ->  Base-spacecraft  twist  vector 

%  tm  ->  Manipulator  twist  vector. 

%  Bij  ->  Twist-propagation  matrix  (for  manipulator  i>0  and  j>0) . 

%  BiO  ->  Twist-propagation  matrix  (for  i>0  and  j=0) . 

%  PO  ->  Base-spacecraft  twist-propagation  vector. 

%  pm  ->  Manipulator  twist-propagation  vector. 

%  TEE  ->  End-Effector  Homogeneous  transformation  matrix. 

%===  LICENSE  ===% 


%===  CODE  ===s> 


% -  Number  of  links  and  Joints  - % 

n=data . n; 

% -  Homogeneous  transformation  matrices  - % 
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%Pre-allocate  homogeneous  transformations  matrices 
TJ=zeros (4, 4 ,  n+1 )  ; 

TL=zeros (4, 4, n) ; 

%Base-spacecraft 

TO  =  [RO,  rO;  zeros  (1, 3)  ,  1]  ; 

%First  Joint 

TJ (1:4, 1:4,1)  =T0*data . base . T_L0_J1 ; 

%Forward  recursive  for  rest  of  joints  and  links 
for  i=l : n 

%Compute  Rotation  matrix  and  translation  vector  from  DH  parameters 
[R, s]  =  DH_Rs (data . man ( i ). DH, qm ( i ), data . man ( i ). type ) ; 

%Compute  joint  homogeneous  transformation  matrix 
T J ( 1 : 4 , 1 : 4 , i+1 ) =T J (1:4,1 : 4,  i) * [R,  s;  zeros (1, 3)  ,  1]  ; 

%Compute  link  homogeneous  transformation  matrix 

TL(l:4,l:4,i)=TJ(l:4,l:4,i+l)* [eye ( 3 ) , -data . man (i).b;  zeros  ( 1 , 3 ) , 

1] ;  %homog  trans  matrix  of  last  link 
end 

%End-Ef f ector 

TEE  =  T J ( 1 : 4 , 1 : 4 ,  n+1 )  ;  %3x3  rotation  matrix  plus  3x1  translation  vector 
describing  position  of  EE 

% -  Rotation  matrices,  translation,  position  and  other  geometry 

vectors - % 

%Pre-allocate  rotation  matrices,  translation  and  position  vectors 
RJ=zeros (3, 3, n) ; 

RL=zeros (3, 3, n) ; 
r=zeros (3, n) ; 
l=zeros (3, n) ; 

%Pre-allocate  axis 
e=zeros (3, n) ; 

%Pre-allocate  other  gemotery  vectors 
g=zeros (3, n) ; 

%Format  Rotation  matrices,  link  positions,  joint  axis  and  other 

geometry 

%vectors 

for  i=l : n 

RJ (1 : 3, 1 : 3, i) =TJ (1:3, 1 : 3, i) ; 

RL ( 1 : 3 , 1 : 3 , i ) =TL ( 1 : 3 , 1 : 3 , i ) ; 
r  (1 :  3,  i)  =TL  (1 :  3,  4,  i)  ; 
e  (1 :  3,  i)  =RJ  (1 :  3,  3,  i)  ; 

1 (l:3,i)=TJ (1:3, 4 , i ) ; 
g (1 : 3,  i) =r (1 : 3, i) -1  (1:3, i) ; 

end 

% -  Twist-propagtaion  matrix  - % 

%Pre-allocate  Bij 
Bij=zeros (6, 6, n, n) ; 

%Compute  Bij 
for  j=l : n 

for  i=l : n 

Bi j (1 : 6, 1 : 6, i, j ) = [eye (3) ,  zeros(3,3);  SkewSym (r (1 : 3, j ) - 
r (1 : 3, i) )  ,  eye (3) ]  ; 
end 

end 

%Pre-allocate  BiO 
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BiO=zeros (6, 6, n) ; 

%Compute  BiO 
for  i=l : n 

BiO (1 : 6, 1 : 6, i) = [eye (3) ,  zeros(3,3);  SkewSym (rO-r (1 : 3, i) ) ,  eye  (3)]; 

end 

% -  Twist-Propagation  vector  - % 

%Pre-al locate 
pm=zeros ( 6, n) ; 

%Base-spacecraft 

P0= [RO, zeros (3, 3) ;  zeros(3,3),  eye (3)]; 

%Fordward  recursion  to  obtain  the  Twist-Propagation  vector 
for  i=l : n 

if  data . man ( i ). type==0 
%Revolute  joint 

pm (l:6,i)  =  [e(l:3,i) ; cross (e (1 : 3, i)  ,g(l:3,i) ) ]  ; 

else 

%Prismatic  joint 

pm (1:6, i)  . p=[zeros(3,l) ;e (1 :3,  i) ]  ; 

end 

end 

% -  Generalized  twist  vector  - % 

%Pre-Al locate 
tm=zeros ( 6, n)  ; 

%Base-spacecraft 

t0=P0*q0dot; 

%First  link 

tm  (1:6, 1) =BiO  (1 : 6, 1:6,1) *tO+pm (1:6,1) *qmdot (1)  ; 

%Fordward  recursion  to  obtain  the  twist  vector 
for  i=2 : n 

tm  (1 : 6, i) =Bi j  (l:6,l:6,i,i-l) *tm (1 : 6, i-1) +pm (1 : 6,  i) *qmdot (i)  ; 

end 

end 
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APPENDIX  G.DH  RS.M 


%  This  function  computes  the  rotation  matrix  R  and  the  translation 
vector  s 

%  between  two  joints  given  their  Denavit-Hartenber  (DH)  parameters. 

%  Developed  by  Dr.  Josep  Virgili-Llop 

function  [R, s]  =  DH_Rs (DH, qm, type) 

"6 

%  Inputs: 

%  DH  ->  Denavit-Hartenberg  parameters. 

%  DH.d  ->  Distance  between  joint  origins  along  the  joint  z-axis . 

%  DH. theta  ->  Rotation  between  x-axis  along  the  joint  z-axis. 

%  DH. alpha  ->  Rotation  between  z-axis. 

%  DH.a  ->  Distance  between  the  commaon  normal  between  the  z-axis 

%  qm  ->  Joint  variable. 

%  type  ->  type==0  for  revolute  joint  or  type==l  for  prismatic  joints 

'o 

%  Outputs: 

%  R  ->  Rotation  3x3  matrix. 

%  s  ->  Translation  3x1  vector. 

%===  LICENSE  ===% 

%===  CODE  ===% 

%Assign  the  d  and  theta  variable  depending  on  joint  type 
if  type==0 

%Revolute  joint 
theta=qm; 
d=DH . d; 

else 

%Prismatic  joint 
theta=Dh . theta; 
d=qm; 

end 


%Rotation  matrix 
R  =  [ 

cos (theta) ,  -sin (theta) *cos (DH . alpha) , 
sin (theta) ,  cos (theta) *cos (DH . alpha) , 
0,  sin  (DH . alpha) , 

]  ; 


sin (theta) *sin (DH . alpha) ; 
-cos (theta) *sin (DH . alpha) ; 
cos (DH . alpha) 


%Translation  vector 

s  =  [DH . a*cos (theta) ,  DH . a*sin (theta) ,  d] ' ; 
end 
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o\°  o\° 


APPENDIX  H.  SKEWSYM.M 


SkewSym.m  computes  the  skewsymmetric  matrix  of  a  vector 
Developed  by  Dr.  Josep  Virgili-Llop 

function  [x_skew]  =  SkewSym(x) 

x_skew=[0  -x(3)  x(2)  ;  x(3)  0  -x(l)  ;  -x(2)  x(l)  0  ]; 

end 
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APPENDIX  I.  MARSMAN  KINCAL.M 


%MARSMAN_KinCal . m  calibrates  the  kinematic  model  based  on  actually 

measured  end  effector 

%positions 

%Developed  by  Dr.  Josep  Virgili-Llop  and  CPT  Jerry  Drew 


%  Inputs: 

%  q  ->  measured  angular  positions  of  each  joint 
%  delta_a  ->  induced  variation  in  DH.a 
%  delta_theta  ->  induced  variation  in  DH. theta 
%  delta_q  ->  induced  variation  in  joint  position 

%  delta_x  ->  induced  variation  in  x  direction  displacement 

%  DH  ->  Denavit-Hartenberg  parameters. 

%  DH.d  ->  Distance  between  joint  origins  along  the  joint  z-axis . 

%  DH. theta  ->  Rotation  between  x-axis  along  the  joint  z-axis. 

%  DH. alpha  ->  Rotation  between  z-axis. 

%  DH.a  ->  Distance  between  the  commaon  normal  between  the  z-axis. 

%  EE_Eul_Ang  ->  angular  component  of  vector  from  origin  of  inertial 
frame  to  end 

%  effector,  scalar  (rad) 

%  Eps  ->  convergence  error  tolerance  for  calibration 
%  I  ->  inertia  of  manipulator  links 
%  qm  ->  Joint  variable. 

%  mass  ->  mass  of  manipulator  links  (kg) 

%  PHI  ->  kinematic  calibration  matrix 

%  r_EE_x  ->  x  component  of  vector  from  origin  of  inertial  frame  to 


end 


r_EE_y 


effector,  scalar  (m) 

->  y  component  of  vector  from  origin  of  inertial  frame  to 


end 


effector,  scalar  (m) 

RBJ1  ->  rotation  matrix  about  z  of  joint  1  with  respect  to  the  base 
theta_base_Jl  ->  angular  displacement  of  base  with  respect  to  joint 

(inertial  frame) 

type  ->  type==0  for  revolute  joint  or  type==l  for  prismatic  joints. 
x_base_Jl  ->  displacement  of  base  with  respect  to  joint  1  in  x 
direction  (inertial  frame) 

y_base_Jl  ->  displacement  of  base  with  respect  to  joint  1  in  y 
direction  (inertial  frame) 


Outputs : 

Delta_Zeta_Direct  ->  matrix  containing  computed  variations  between 

measured  and  actual  DH  parameters 


clc 
clear 
close  all 
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%Load  Data 

load  ( ' Kinematic_Data/Calibration_data_March3 .mat ' ) 

load  ( ' Kinemat ic_Data/Vicon_MARSMAN_CAL_2 0160303T111525. mat ' ) 

% -  Prepare  data - % 

q_meas=q; 

q_meas ( 3 : 4 , : )  =  [];  %Remove  unwanted  measurements 

q= [ ] ;  %Delete  this  variable  as  it  is  going  to  be  used  later  on. 


% -  Define  manipulator  data  - % 

%Number  of  joints/links 
data . n=4 ; 

%First  joint 
data.man(l) .type=0; 
data.man(l) . DH.d  =  0; 
data . man ( 1 ). DH . alpha  =  0; 

data . man ( 1 ) . DH . a  =  .39;  %length  from  joint(i)  to  joint (i+1),  meters 

data . man ( 1 ). b  =  [data . man ( 1 ). DH . a/2 ; 0 ; 0 ]  ; 

data.man(l) ,mass=2.88; 

data . man (1) . I=eye ( 3 ) *0 . 04 ; 

data.man(l) ,q0=0; 

%Second  joint 

data . man (2 ) =data . man ( 1 ) ; 

%Third  joint 

data . man ( 3 ) =data . man ( 1 ) ; 

%Fourth  joint 

data . man ( 4 ) =data . man ( 1 )  ; 


%Base  to  first  link  data  (initial  guess) 
theta_base_Jl  =  0; 
x_base_Jl  =  0.2; 
y_base_Jl  =  0; 

%First  joint  location  with  respect  to  base 

RB Jl= [cos (theta_base_Jl )  -sin (theta_base_Jl )  0;  %Rotation  matrix 

about  z 

sin (theta_base_Jl )  cos (theta_base_Jl )  0; 

0  0  1]  ; 
data . base . T_L0_J1= [RB J1 , [x_base_Jl ; y_base_Jl ; 0 ] ; zeros (1,3) , 1 ] ; 

%Base-spacecraft  inertia  matrix 

data ,base.mass=12; 

data . base . I=eye ( 3 ) *0.22; 

% - LSQ  Parameters - % 

Eps  =  0.01;  %  convergence  error  tolerance 

128 


Delta_Zeta_Direct  =  2*Eps; 

%Variations 

delta_d  =  le-3;  %Distance  variation 
delta_theta  =  deg2rad ( . 0 1 ) ;  %Angle  variation 

%Initial  guess  on  joint  offsets 
for  j=l : size (q_meas, 1 ) 
for  i=l : data . n 

q ( j  ,  i ) =q_meas ( j , i ) +data . man ( i )  . qO ; 

end 

end 

% -  Iterative  Batch  LSQ  - % 

while  max (abs (Delta_Zeta_Direct) )  >  Eps; 

%Nominal  Values 

[rEE_x,  rEE_y,  rEE_theta]  =  x_EE (qO_meas,  q,  data);  %  kinematic 
model  estimate  of  end  effector  state 

% -  Derivatives  for  DH . a  for  all  links  except  the  last  one  - % 

%It  is  assumed  that  all  links  have  the  same  length  (except  the  last 

one) 

%Copy  Manipulator  data 
data_var=data; 

%Change  DH.a 

for  i  =  l:data.n-l 

data_var . man ( i ) .DH.a  =  data.man(i) .DH.a  +  delta_d; 

end 

%Values  with  varied  parameters 

[rEE_x_delta,  rEE_y_delta,  rEE_theta_delta]  =  x_EE (qO_meas ,  q, 
data_var) ;  %  compute  position  of  EE  under  variation 
%Partial  derivatives  matrix 

PHI(:,1)  =  [ ( (rEE_x_delta  -  rEE_x) /delta_d) ( (rEE_y_delta  - 
rEE_y) /delta_d)  ' ;  (rEE_theta_delta ( : ) -rEE_theta ( : ) ) /delta_d]  ; 

% -  Derivatives  for  DH.a  for  the  last  link  - % 

%Copy  Manipulator  data 
data_var=data; 

%Change  DH.a 

data_var . man (end) .DH.a  =  data . man (end) .DH.a  +  delta_d; 

%Values  with  varied  parameters 

[rEE_x_delta,  rEE_y_delta,  rEE_theta_delta]  =  x_EE (qO_meas ,  q, 
data_var) ;  %  compute  position  of  EE  under  variation 

%Partial  derivatives  matrix 

PHI(:,2)  =  [ ( (rEE_x_delta  -  rEE_x) /delta_d) ( (rEE_y_delta  - 
rEE_y) /delta_d) ' ;  (rEE_theta_delta ( : ) -rEE_theta ( : ) ) /delta_d] ; 

% -  Derivatives  with  respect  to  Base  to  First  Joint  parameters  - 

"o 


%Angle 

data_var=data; 

theta_base_Jl_var  =  theta_base_Jl  +  delta_theta; 
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RBJ1= [cos (theta_base_Jl_var )  -sin (theta_base_Jl_var )  0;  %Resets 

RBJ1  to  baseline 

sin (theta_base_Jl_var )  cos (theta_base_Jl_var )  0; 

0  0  1]  ; 

data_var . base . T_L0_J1 ( 1 : 3, 1 : 3 ) =RB J1 ; 

%Values  with  varied  parameters 

[rEE_x_delta,  rEE_y_delta,  rEE_theta_delta]  =  x_EE (q0_meas ,  q, 
data_var) ;  %  compute  position  of  EE  under  variation 

%Partial  derivatives  matrix 

PHI(:,3)  =  [ ( (rEE_x_delta  -  rEE_x) /delta_theta) ' ;  ( (rEE_y_delta  - 

rEE_y) /delta_theta) ' ;  (rEE_theta_delta ( : ) -rEE_theta ( : ) ) /delta_theta] ; 

%X  position  of  first  joint 
data_var=data; 

data_var . base . T_L0_J1 ( 1 , 4 ) =x_base_Jl  +  delta_d; 

%Values  with  varied  parameters 

[rEE_x_delta,  rEE_y_delta,  rEE_theta_delta]  =  x_EE (q0_meas ,  q, 
data_var) ;  %  compute  position  of  EE  under  variation 

%Partial  derivatives  matrix 

PHI(:,4)  =  [ ( (rEE_x_delta  -  rEE_x) /delta_d) ( (rEE_y_delta  - 
rEE_y) /delta_d) ' ;  (rEE_theta_delta ( : ) -rEE_theta ( : ) ) /delta_d] ; 

%X  position  of  first  joint 
data_var=data; 

data_var . base . T_L0_J1 (2 , 4 ) =y_base_Jl  +  delta_d; 

%Values  with  varied  parameters 

[rEE_x_delta,  rEE_y_delta,  rEE_theta_delta]  =  x_EE (q0_meas ,  q, 
data_var) ;  %  compute  position  of  EE  under  variation 

%Partial  derivatives  matrix 

PHI(:,5)  =  [ ( (rEE_x_delta  -  rEE_x) /delta_d) ( (rEE_y_delta  - 
rEE_y) /delta_d) ' ;  (rEE_theta_delta ( : ) -rEE_theta ( : ) ) /delta_d] ; 

%Joint  angle  Offsets 
for  i  =  2 : data . n 
data_var=data; 

data_var . man ( i ) . qO=data_var . man ( i ) . qO+delta_theta; 
q_var=q; 

for  j  =  l : size (q_meas,  1 ) 

q_var ( j , i ) =q_meas ( j , i ) +data_var . man ( i ) . qO ; 

end 

%Values  with  varied  parameters 

[rEE_x_delta,  rEE_y_delta,  rEE_theta_delta]  =  x_EE (q0_meas , 
q_var,  data_var) ;  %  compute  position  of  EE  under  variation 
%Partial  derivatives  matrix 

PHI(:,i+5-l)  =  [ ( (rEE_x_delta  -  rEE_x) /delta_theta)  ' ; 

( (rEE_y_delta  -  rEE_y) /delta_theta) ' ;  (rEE_theta_delta ( : ) - 
rEE_theta ( : ) ) /delta_theta] ; 
end 

% -  Create  Delta_x  - % 

x_n  =  [rEE_x'  ;  rEE_y ' ;  rEE_theta']; 

x_m  =  [EE_meas ( : , 1) ;  EE_meas ( : , 2) ;  EE_meas ( : , 3) ] ; 

Delta_x  =  x_m  -  x_n; 
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% - Solve  LSQ - % 

Delta_Zeta_Direct  =  inv (PHI ' *PHI) *PHI ' *Delta_x; 


% -  Update  parameters  - % 

%Update  links  lengths 
for  i  =  l:data.n-l 

data . man ( i )  . DH . a  =  data . man ( i )  . DH . a  +  Delta_Zeta_Direct ( 1 )  ; 

end 

data . man (end) . DH . a  =  data . man (end) . DH . a  +  Delta_Zeta_Direct (2 ) ; 
%Update  position  of  first  joint  w.r.t  base 
theta_base_Jl  =  theta_base_Jl+  Delta_Zeta_Direct ( 3 ) ; 
x_base_Jl  =  x_base_Jl  +  Delta_Zeta_Direct ( 4 ) ; 
y_base_Jl  =  y_base_Jl  +  Delta_Zeta_Direct ( 5 ) ; 

RBJ1= [cos (theta_base_Jl )  -sin (theta_base_Jl )  0; 

sin (theta_base_Jl )  cos (theta_base_Jl )  0; 

0  0  1]  ; 
data . base . T_L0_J1= [RB J1 , [x_base_Jl ; y_base_Jl ; 0 ] ; zeros (1,3),!]; 
%Joint  offsets 
for  i  =  2 : data . n 

data . man ( i ) . q0=data . man ( i ) . qO+Delta_Zeta_Direct ( 5+i-l ) ; 

end 

for  j=l : size (q_meas,  1 ) 
for  i=l : data . n 

q ( j , i ) =q_meas ( j , i ) fdata . man ( i )  . qO  ; 

end 

end 


end 

% - Print  Results - % 

for  i  =  1 : data . n 

fprintf ( ' Link  %i  DH  a:  %f  m\n ' , i, data . man ( i ) . DH . a) ; 

end 

for  i  =  1 : data . n 

fprintf (' Link  %i  Joint  Offset:  %f 
deg\n ' , i, rad2deg (data . man ( i ) . qO ) ) ; 
end 

fprintf ('L0  to  J1  parameters  [x,y,  theta]  :  %f  m,  %f  m,  %f 
deg . \n ' , x_base_Jl , y_base_Jl , rad2deg (theta_base_Jl ) )  ; 

% -  Print  Residuals  - % 

fprintf ( ' \n ' ) ; 

%Compute  kineamtics  with  last  iteration 

[rEE_x,  rEE_y,  rEE_theta]  =  x_EE (q0_meas,  q,  data); 

%Compute  residuals  (mean  and  std) 

x_res= (rEE_x ' -EE_meas ( : , 1 ) ) ; 

y_res= (rEE_y ' -EE_meas ( :  ,  2 ) ) ; 

pos_res  =  sqrt (x_res . A2+y_res . A2 ) ; 

fprintf (' position  residuals  [mean, std]:  %f,  %f 

m. \n ' ,mean (pos_res) , std (pos_res) ) ; 

%Compute  residuals  (mean  and  std) 
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theta_res= (rEE_theta ' -EE_meas ( : , 3) ) ; 

fprintf (' theta  residuals  [mean, std] :  %f,  %f 

deg . \n ' , rad2deg (mean (theta_res ) ) , rad2deg ( std (theta_res ) ) ) ; 

% - Update  b  vector - % 

for  i  =  1 : data . n 

data . man ( i ). b  =  [data . man ( i ). DH . a/2 ; 0 ; 0 ] ; 

end 

% -  Save  manipulator  Data  (ready  for  dynamic  calibration) 

save ( ' KinCal_ManData '  ,  ' data ' ) ; 
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APPENDIX  J.  X_EE.M 


%Comupte  the  state  of  the  end-effector  according  to  the  kinematic  model 
and 

%the  joint  states  (base  and  manipulator  joints) . 

%  Developed  by  Dr.  Josep  Virgili-Llop  and  CPT  Jerry  Drew 

function  [rEE_x,  rEE_y,  rEE_theta]  =  x_EE (qO_meas,  q,  data) 

%Iterate  throught  the  different  states 
for  i  =  1: length (q) 

%Base  rotation  matrix 
R0= [cos (qO_meas (i, 3) ) 
sin (qO_meas (i, 3) ) 

0 

%Base  position 
r0= [qO_meas (i, 1) ; 
qO_meas (i, 2) ; 

0]  ; 

%Joint  variables 

qm  =  q(i,  :) ;  %manipulator  joint  states  [m  x  n]  where  m  is  the 
number  of  measurements  taken  and  n  is  the  number  of  links 

% -  Compute  Kinematics,  Dynamics,  ID,  and  FD  - % 

%Kinematics 


%translation  in  x 
%translation  in  y 
%translation  in  z 


-sin (q0_meas (i, 3) )  0; 

cos (q0_meas (i, 3) )  0; 

0  1]  ; 


l~  r  ~  r  ~  r  ~  r  ~  r  ~  r  ~  r  ~  r 

(data . n, 1 ) , data) ; 

rEE_theta  ( i ) 
inertial  frame 

rEE_x ( 1 , i )  = 
rEE_y ( 1 , i )  = 


~, ~, ~, TEE] =Kinematics_Serial (R0,r0,qm,  zeros (6,1)  ,  zeros 

=  dcm2angle (TEE (1 : 3, 1 : 3) ' ) ;  %orientations  of  EE  wrt 

TEE (1, 4)  ; 

TEE (2,4)  ; 


end 
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APPENDIX  K.  MARSMAN  DYNCAL.M 


%  Performs  the  dynamic  calibration  based  upon  the  dynamic  model, 

%  measurement  data,  and  the  results  of  the  kinematic  calibration. 

%  Developed  by  Dr.  Josep  Virgili-Llop  and  CPT  Jerry  Drew 

%  Inputs: 

%  delta_d  ->  distance  variation  of  b  vector 
%  delta_m  ->  mass  variation  (kg) 

%  delta_I  ->  inertia  variation  (kg*mA2) 

%  Delta_x  ->  difference  between  measured  and  nominal  base  angular 
position 

%  Eps  ->  convergence  error  tolerance  for  calibration 
%  qml  ->  joint  positions  before  maneuver 

%  qm2  ->  joint  positions  after  maneuver 

%  qOl  ->  state  (x,y, theta)  of  base  before  maneuver 

%  q02  ->  state  (x,y, theta)  of  base  after  maneuver 

%  qO_px  ->  nominal  (predicted)  x  value  of  base  from  dynamic  model 

%  qO_py  ->  nominal  (predicted)  y  value  of  base  from  dynamic  model 

%  qO_ptheta  ->  nominal  (predicted)  theta  value  of  base  from  dynamic 
model 

%  xEEl  ->  end  effector  state  before  maneuver 

%  xEE2  ->  end  effector  state  after  maneuver 

%  x_m  ->  measured  theta  value  of  base 

%  x_n  ->  nominal  (predicted)  theta  value  of  base  from  dynamic  model 
%  (qO_ptheta') 

%  I  ->  inertia  of  manipulator  links 
%  PHI  ->  dynamic  calibration  matrix 

%  Outputs: 

%  Delta_Zeta_Direct  ->  matrix  containing  parameter  variations  with 
%  respect  to  the  nominal  values  (Siciliano,  p. 

89)  ; 

%  the  solution  to  the  iterative  least-squares 

%  problem 

clc 
clear 
close  all 

%Load  Data 

load  ( ' KinCal_ManData . mat ' )  %loads  results  from  kinematic  calibration 
for  i  =  1:15 

load  (sprintf ( ' DynCal_Data/qmat%i .mat ' , i) )  %  loads  telemetry  joint 
measurements 

load  ( sprintf (' DynCal_Data/Man_%i . mat ', i ) )  % 

EE  measurements 


%Initial  guess  on  joint  offsets 
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loads  VICON  base  and 


for  j=l : size (q, 1 ) 
for  k=l : data . n 

q ( j  ,  k) =q ( j , k) +data . man (k)  . qO ;  %  accounts  for  offsets 
determined  by  kinematic  calibration 
end 

end 


maneuver (1) . qOl  =  qO_meas  (1, 
beginning  of  maneuver 


maneuver (i) ,q02  = 

q0_meas 

(2, 

end  of  maneuver 

maneuver (i) . qml  = 

q  (1,  :)  ; 

"6 

maneuver (i) . qm2  = 

q  (2,  :  )  ; 

"6 

maneuver (i) . xEEl 

=  EE_meas 

(l 

maneuver 

maneuver (i) ,xEE2 

=  EE_meas 

(2 

maneuver 


1:3);  %  state  (x,y, theta)  of  base  at 

1:3);  %  state  (x,y, theta)  of  base  at 

joint  positions  before  maneuver 
joint  positions  after  maneuver 
,1:3);  %  end  effector  state  before 

,1:3);  %  end  effector  state  after 


end 

maneuver ( [6,7,8, 15] )=[] ; 
maneuver ([4,8,10,11])=[]; 

% - LSQ  Parameters - % 

Eps  =  0.01;  %  convergence  error  tolerance 
Delta_Zeta_Direct  =  2*Eps; 

%Variations 

delta_d  =  le-3;  %  Distance  variation  of  b  vector 
delta_m  =  le-3;  %  Mass  variation  (kg) 
delta_I  =  le-5;  %  Inertia  variation  (kg*mA2) 


% -  Iterative  Batch  LSQ  - % 

while  max (abs (Delta_Zeta_Direct) )  >  Eps; 

%Nominal  Values 

[q0_px,  q0_py,  q0_ptheta]  =  qO_maneuver (maneuver,  data);  %  nominal 
(predicted)  values  from  dynamic  model 

% -  Derivatives  for  DH . I  for  all  links  - % 

%Copy  Manipulator  data 
data_var=data; 

%Change  DH . I 
for  i  =  1 : data . n 

data_var .man (i)  . I  (3, 3)  =  data .man (i)  . I  (3, 3)  +  delta_I;  % 
changing  z  component  of  Inertia  matrix  (in  local  frame) 
end 

%Values  with  varied  parameters 

[qO_px_delta,  qO_py_delta,  qO_ptheta_delta]  =  qO_maneuver (maneuver, 
data_var) ; 

PHI(:,1)  =  [ ( (qO_ptheta_delta  -  q0_ptheta) /delta_I ) ' ] ; 
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% -  Derivatives  for  DH.I  for  the  last  link  - % 

%Copy  Manipulator  data 
data_var=data; 

data_var .man (end)  . I (3,  3)  =  data .man (end)  . I  (3, 3)  +  delta_I; 


%  %Values  with  varied  parameters 

%  [qO_px_delta,  qO_py_delta,  qO_ptheta_delta]  = 

qO_maneuver (maneuver,  data_var) ; 

%  PHI(:,2)  =  [ ( (qO_ptheta_delta  -  qO_ptheta) /delta_I ) ' ] ; 


% -  Create  Delta_x  - % 

x_n  =  qO_ptheta'; 

for  i=l : length (maneuver ) 

x_mx ( i, 1 ) =maneuver ( i ) . q02 ( 1 ) ; 
x_my ( i, 1 ) =maneuver ( i ) . q02 (2 ) ; 
x_mtheta ( i, 1 ) =maneuver ( i ) . q02 ( 3 ) ; 

end 

x_m  =  x_mtheta; 

Delta_x  =  wrapToPi (x_m  -  x_n)  ; 

% - Solve  LSQ - % 

Delta_Zeta_Direct  =  inv (PHI ' *PHI ) *PHI ' *Delta_x; 


% -  Update  parameters  - % 

%Update  links  Inertia 
for  i  =  1 : data . n 

data .man  (i)  . I (3, 3)  =  data .man  (i)  . I (3, 3)  + 
Delta_Zeta_Direct ( 1 )  /10  ; 
end 

end 

% - Print  Results - % 

for  i  =  1 : data . n 

fprintf ( ' Link  %i  I:  %f  kg  mA2\n ' , i, data .man  (i)  . I  (3, 3) ) ; 

end 

for  i  =  1 : data . n 

fprintf (' Link  %i  b:  %f  m\n i, data . man ( i ). b ( 1 )) ; 

end 

% - Print  Residuals - % 

fprintf ( ' \n ' ) ; 

%Compute  kineamtics  with  last  iteration 

[qO_px,  qO_py,  qO_ptheta]  =  qO_maneuver (maneuver,  data); 
%Compute  residuals  (mean  and  std) 
x_n  =  qO_ptheta'; 

qO_theta_res=  rad2deg (wrapToPi (x_m  -  x_n) ) ; 
fprintf (' position  residuals  [mean, std] :  %f,  %f 
deg . \n '  ,  mean (qO_theta_res ) , std (qO_theta_res ) ) ; 
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% -  Save  manipulator  Data  (ready  for  dynamic  calibration) 

save ( ' DynCal_ManData ' , ' data ' ) ; 
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APPENDIX  L.  I  I.M 


%  Converts  the  inertias  in  local  frame  to  inertia  in  the  inertial 
frame . 

%  Developed  by  Dr.  Josep  Virgili-Llop 

function  [10,  Im] =I_I (RO, RL, data) 

%  Input  -> 

%  RO  ->  Rotation  matrix  from  the  base-spacecraft  to  the  inertial 
frame . 

%  RL  ->  Links  3x3  rotation  matrices. 

%  data.n  ->  Manipulator  number  of  joints  and  links. 

%  data. man  ->  Manipulator  data. 

%  data. man (i) .1  ->  Link  inertia. 

%  Output  -> 

%  10  ->  Base-spacecraft  inertia  in  inertial  frame. 

%  Im  ->  Manipulator  inertia  in  inertial  frame. 

%===  LICENSE  ===% 

%===  CODE  ===% 

%Base-spacecraft  inertia 
10  =  R0*data . base . I ; 

%Pre-allocate  inertias 
Im=zeros (3,3,  data . n) ; 

%Inertias  of  the  links 
for  i=l : (data . n) 

Im(l:3,l:3,i) =RL (1 : 3, 1 : 3,  i) *data .man (i)  . I*RL (1 : 3, 1 : 3,  i) 

end 

end 
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APPENDIX  M.  MCB  SERIAL.M 


%  Computes  the  Mass  Composite  Body  matrix  of  a  Serial  Manipulator. 

%  Developed  by  Dr.  Josep  Virgili-Llop 

function  [M0_tilde, Mm_tilde] =MCB_Serial (10, Im, Bi j, BiO, data) 

%  Input  -> 

%  10  ->  Base-spacecraft  inertia  in  inertial  frame. 

%  Im  ->  Manipulator  inertia  in  inertial  frame. 

%  Bij  ->  Twist-propagation  matrix  (for  manipulator  i>0  and  j>0) . 

%  BiO  ->  Twist-propagation  matrix  (for  i>0  and  j=0) . 

%  data. base  ->  Base-spacecraft  data 

%  data . base . mass  ->  Base-spacecraft  mass. 

%  data. man  ->  Manipulator  data. 

%  data. man. n  ->  Manipulator  number  of  joints  and  links. 

%  data. man (i) .mass  ->  Link  mass. 

%  Output  -> 

%  M0_tilde  ->  Base-spacecraft  mass  matrix  of  composite  body. 

%  Mm_tilde  ->  Manipulator  mass  matrix  of  composite  body. 

%===  LICENSE  ===% 

%===  CODE  ===% 

%Number  of  links  and  Joints 
n=data . n; 

%Pre-al locate 
Mm_tilde=zeros (6, 6, n) ; 

%Initialize  M  tilde 

Mm_tilde (l:6,l:6,n)=[Im(l:3,l:3,n),zeros(3,3);zeros(3,3), data . man (n) . ma 
ss*eye  ( 3 ) ] ; 

%Backwards  recursion 
for  i=n-l : -1 : 1 

Mm_tilde ( 1 : 6, 1 : 6, i) = [Im (1 : 3, 1 : 3, i ) , zeros (3, 3) ; zeros (3, 3) , data . man (i ) . ma 
ss*eye (3) ] +Bi j (1 : 6, 1 : 6, i+1, i) ' *Mm_tilde (l:6,l:6,i+l)*Bij(l:6,l:6,i+l,i) 

r 

end 

%Base-spacecraft  M  tilde 

M0_tilde= [10, zeros (3, 3) ; zeros (3, 3) , data ,base.mass*eye (3) ] +BiO (1 : 6,  1 : 6,  1 
)  ' *Mm_tilde (1 : 6,  1 : 6,  1) *BiO (1 :  6,  1 :  6,  1)  ; 


end 
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APPENDIX  N.  GIM  SERIAL.M 


%  Computes  the  Generalized  Inertia  Matrix  of  a  Serial  Manipulator 
%  Developed  by  Dr.  Josep  Virgili-Llop 

function  [HO,  HOm,  Hm]  = 

GIM_Serial (M0_tilde, Mm_tilde, Bij,BiO,PO, pm, data) 

"o 

%  Input  -> 

%  M0_tilde  ->  Base-spacecraft  mass  matrix  of  composite  body. 

%  Mm_tilde  ->  Manipulator  mass  matrix  of  composite  body. 

%  Bij  ->  Twist-propagation  matrix  (for  manipulator  i>0  and  j>0) 

%  BiO  ->  Twist-propagation  matrix  (for  i>0  and  j=0) . 

%  PO  ->  Base-spacecraft  twist-propagation  vector. 

%  pm  ->  Manipulator  twist-propagation  vector. 

%  data  ->  Manipulator  data. 

%  data.n  ->  Manipulator  number  of  joints  and  links. 

"6 

%  Output  -> 

%  HO  ->  Base-spacecraft  inertia  matrix. 

%  HOm  ->  Base-spacecraft  -  manipulator  coupling  inertia  matrix. 
%  Hm  ->  Manipulator  inertia  matrix. 

%===  LICENSE  ===% 


%===  CODE  ===% 

% -  Number  of  links  and  Joints  - % 

n=data . n; 

% - H  Martix - % 

%Base-spacecraft  Inertia  matrix 
HO  =  PO ' *M0_tilde*P0 ; 

%Pre-allocate  Hm 
Hm=zeros (n,  n) ; 

%Manipulator  Inertia  matrix  Hm 
for  j=l : n 

for  i= j : n 

Hm ( i, j ) =pm ( 1 : 6, i )  ' *Mm_tilde (1 : 6, 1 : 6, i) *Bi j ( 1 : 6, 1 : 6, i, j ) *pm  (1:6, j ) 
Hm ( j , i ) =Hm  ( i , j ) ; 

end 

end 

%Pre-allocate  HOm 
H0m=zeros ( 6, n) ; 

%Coupling  Inertia  matrix 
for  i=l : n 

HOm ( 1 : 6, i)  =  (pm (1 : 6, i)  '  *Mm_tilde (1 : 6,  1 : 6, i) *BiO (l:6,l:6,i)*P0) 

end 

end 
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APPENDIX  O.  Q_DOT_FUN.M 


%  Computes  time  derivatives  of  the  base-spacecraft  state  vector  (q_dot) 
%  Developed  by  Dr.  Josep  Virgili-Llop 


function 

"6 

%  Inputs 
%  Bi  j 

%  BiO 
%  HO 
%  HOm 
%  Hm 
%  10 
%  Im 
%  M0_t 
%  Mm_t 
%  P0 
%  pm 
%  R0 
frame . 

%  rO 


[q_dot]  =  q_dot_fun (t, q, qmdot, maneuver, data) 


->  Twist-propagation  matrix  (for  manipulator  i>0  and  j > 0 )  . 
->  Twist-propagation  matrix  (for  i>0  and  j=0) . 

>  Base-spacecraft  inertia  matrix. 

->  Base-spacecraft  -  manipulator  coupling  inertia  matrix. 

>  Manipulator  inertia  matrix. 

>  Base-spacecraft  inertia  in  inertial  frame. 

>  Manipulator  inertia  in  inertial  frame. 

ilde  ->  Base-spacecraft  mass  matrix  of  composite  body, 
ilde  ->  Manipulator  mass  matrix  of  composite  body. 

>  Base-spacecraft  twist-propagation  vector. 

>  Manipulator  twist-propagation  vector. 

>  Rotation  matrix  from  the  base-spacecraft  to  the  inertial 

>  Position  of  the  base-spacecraft  to  the  inertial  frame. 


%  Outputs: 

%  q_dot  ->  derivative  of  the  state  vector  of  the  spacecraft  base  and 
%  manipulator  joints,  of  the  form  [q0_dot; qm_dot] 


% - Derivative  of  qm % 

q_dot (4 : 3+data . n) =qmdot; 

% - Derivative  of  qO % 

%Base  rotation  matrix 

R0= [cos (maneuver . qOl (3) )  -sin (maneuver . qOl (3) )  0; 

sin (maneuver . qOl (3) )  cos (maneuver . qOl (3) )  0; 

0  0  1]  ; 

%Base  position 

r0= [maneuver . qO 1 ( 1 ) ;  %translation  in  x 
maneuver . qO 1 (2 ) ;  %translation  in  y 
0];  %translation  in  z 

%Kinematics 

[~,  RL,  ~,  ~,  ~,  ~,  ~,Bij,Bi0,P0,  pm,  ~] =Kinematics_Serial (RO , rO , q ( 4 : end) , zeros 
(6, 1 ) , zeros (data . n, 1 ) , data) ; 

%Inertias 

[10,  Im]  =I_I  (R0,  RL,  data)  ; 

%Mass  Composite  Body  matrix 

[M0_tilde,Mm_tilde] =MCB_Serial (10, Im, Bi j, BiO, data) ; 

%Generalized  Inertia  matrix 

[HO,  HOm,  ~]  =  GIM_Serial (M0_tilde, Mm_tilde, Bi j , BiO , P0 , pm, data)  ; 
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qO_dot=  -inv(HO)  *  HOm  *  qmdot ' ; 
q_dot (1:3)  =  qO_dot ([4,5,3]); 

q_dot=q_dot ( : ) ; 


end 
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APPENDIX  P.  QO_MANEUVER.M 


%  qO_maneuver . m  predicts  the  coordinates  of  the  base's  position  in  the 
inertial  frame  by 

%  integrating  the  joint  velocities. 

"6 

%  Developed  by  Dr.  Josep  Virgili-Llop  and  CPT  Jerry  Drew 
"6 

%  Inputs: 

%  maneuver  ->  structure  containing  position  data  of  base,  joints,  and 
%  end  effector  before  and  after  maneuver 

%  data  ->  structure  containing  number  of  links,  DH  parameters,  mass, 

%  inertias,  types  of  joints,  and  initial  state  vector  of  the 

%  system 

'o 

%  Outputs: 

%  qO_px  ->  nominal  position  data  of  base  and  joints  (x  coordinates) 

%  qO_py  ->  nominal  position  data  of  base  and  joints  (y  coordinates) 

%  qO_theta  ->  nominal  position  data  of  base  and  joints  (theta 

coordinates ) 

function  [qO_px,  qO_py,  qO_ptheta]  =  qO_maneuver (maneuver, data) 
Delta_t=5 . 0 ; 

for  i  =  1 : length (maneuver ) 

qmdot  =  (maneuver ( i ). qm2 -maneuver ( i ). qml ) /Delta_t ;  %assume 
denominator  is  t  =  1 

ode_deriv=@ (t, q) q_dot_fun (t, q, qmdot, maneuver (i)  ,  data) ;  %  q= [qO, qm] 
options  =  odeset ( ' RelTol ' , le-12 ,  ' AbsTol ' , le-12 ) ; 

[t, qO ] =ode45 (ode_deriv,  [0:le- 
3 : Delta_t ] , [maneuver ( i ) . qOl , maneuver ( i ) . qml ] , options ) ; 


qO_px ( i ) =q0 (end,  1 )  ; 
qO_py (i) =q0 (end, 2) ; 
qO_ptheta (i) =q0 (end,  3)  ; 


end 

end 
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